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Preface  to  the  Fourth  Edition 


The  aim  of  the  Fourth  Edition  is  the  same  as  that  of  the  past  editions:  to  provide  the 
reader  with  the  tools  needed  to  better  understand  the  fundamental  concepts  behind 
the  design,  analysis,  control,  and  programming  of  robotic  mechanical  systems  at 
large.  The  current  edition  includes  additional  examples  and  exercises.  Furthermore, 
an  updated  account  of  progress  and  trends  in  the  broad  area  of  robotic  mechanical 
systems,  which  continues  developing  at  an  impressive  pace,  is  included  in  Chap.  1. 
However,  a  comprehensive  summary  of  up-to-date  developments  is  not  possible  in 
the  limits  of  a  book  that  stresses  fundamentals.  An  effort  was  made  to  include  an 
overview  of  the  subject,  with  pertinent  references  for  the  details.  Robotic  systems 
that  were  not  even  mentioned  in  the  First  Edition,  namely,  flying  robots,  especially 
drones  and  quadrotors,  are  now  highlighted. 

In  producing  the  Fourth  Edition,  special  attention  was  given  to  the  consistency 
and  accuracy  of  the  presentation.  In  Chap.  4  new  examples  illustrating  the  imple¬ 
mentation  of  the  Denavit-Hartenberg  notation  and  methodology  are  included,  along 
with  a  numerical  example  on  the  inverse-displacement  problem  for  spherical  wrists. 

Some  materials  that  complement  the  book  are  available  on  the  Springer  site 
allocated  to  the  book: 

http :  //w  w  w.  springer,  com/engineering/robotic  s/book/97 8-3-319-01850-8 

Material  posted  therein  includes  code  intended  to  help  better  understand  the 
most  cumbersome  derivations,  and  to  provide  useful  tools  when  working  out  the 
exercises,  or  simply  to  assist  the  curious  reader  in  exploring  alternative  examples  or 
alternative  methods.  Animation  files  and  film  are  also  included. 

An  important  feature  of  the  code  provided  is  that  it  allows  for  either  symbolic 
manipulations,  using  Maple,  or  numerical  computations,  using  Matlab.  The  rough 
estimates  of  the  solutions  to  systems  of  bivariate  equations,  arising  in  various 
chapters,  but  most  intensively  in  Chap.  9,  are  facilitated  by  the  inclusion  of  a  Matlab 
graphic  user  interface.  Further  refinements  of  these  estimates  are  implemented  by 
means  of  a  Newton-Gauss  least-square  approximation  to  an  overdetermined  system 
of  nonlinear  equations,  as  implemented  in  Matlab. 
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The  excellent  work  done  by  Dr.  Kourosh  Etemadi  Zanganeh,  currently  at 
Canmet  (Nepean,  Ontario,  Canada),  when  he  was  a  Ph.D.  candidate  under  the 
author’s  supervision,  was  instrumental  in  completing  the  Second  Edition.  This  work 
comprises  the  development  of  algorithms  and  code  for  the  solution  of  the  inverse 
displacement  problem  of  serial  robots  with  architectures  that  prevent  a  decoupling 
of  the  positioning  from  the  orientation  problems.  The  material  in  Chap.  9,  which 
was  deeply  revised  in  the  Third  Edition  and  remained  virtually  untouched  in  the 
current  edition,  is  largely  based  on  this  work. 

I  would  like  to  thank  all  those  who  provided  valuable  advice  for  improvement: 
Profs.  Carlos  Lopez-Cajun,  Universidad  Autonoma  de  Queretaro  (Mexico),  and  J. 
Jesus  Cervantes-Sanchez,  Universidad  de  Guanajuato  (Mexico),  pointed  out  many 
inconsistencies  in  the  Eirst  Edition;  Dr.  Zheng  Liu,  Canadian  Space  Agency,  St.- 
Hubert  (Quebec,  Canada),  who  taught  a  course  based  on  the  first  six  chapters  of 
the  book  at  McGill  University,  pointed  out  mistakes  and  gave  valuable  suggestions 
for  improving  the  readability  of  the  book.  Additionally,  the  valuable  suggestions 
received  from  Prof.  Pierre  Larochelle,  Elorida  Institute  of  Technology,  were  also 
incorporated.  Needless  to  say,  the  feedback  received  from  students  throughout  more 
than  20  years  of  using  this  material  in  the  classroom  is  highly  acknowledged. 

Not  the  least,  the  C-code  RVS,  developed  on  Silicon  Graphics’  IRIX — a  dialect 
of  UNIX — in  the  1990s,  was  ported  into  Windows.  The  code  is  now  available  under 
the  name  RVS4W  (RVS  for  Windows).  RVS,  introduced  already  in  the  Eirst  Edition, 
is  the  software  system  I  have  used  at  McGill  University’s  Centre  for  Intelligent 
Machines  to  visualize  robot  motions  in  projects  on  design,  control,  and  motion¬ 
planning.  The  original  C-code,  and  the  whole  idea  of  RVS,  is  due  to  the  creative 
work  of  John  Darcovich,  now  a  Senior  Engineer  at  CAE  Electronics  Ltd.,  when 
he  was  a  Research  Engineer  at  McGill  University’s  Robotic  Mechanical  Systems 
Laboratory. 

In  the  Eourth  Edition,  I  include  new  photographs  that  replaced  old  ones.  Eor  the 
magnificent  animation  of  space  robots,  included  in  the  above  site,  I  am  indebted  to 
the  Canadian  Space  Agency  and  MDA,  the  Brampton,  Ontario-based  manufacturer 
of  Canadarm  and  Canadarm2. 

Since  there  is  always  room  for  improvement,  I  welcome  suggestions  from  the 
readership,  to  the  address  below.  Updates  on  the  book  will  be  posted  at 

www.cim.mcgill.ca/~rmsl 

The  Solutions  Manual  has  been  expanded  to  include  more  solutions  of  sampled 
problems.  By  the  same  token,  the  number  of  exercises  has  been  expanded.  The 
manual  is  typeset  in  DTeX  and  contains  numerous  figures;  it  is  available  from  the 
publisher  upon  request. 

In  closing,  I  would  like  to  thank  Dr.  Xiaoqing  Ma,  who  assisted  me  with  the 
editing  of  the  Eourth  Edition  and  the  production  of  a  few  figures.  Dr.  Waseem 
A.  Khan,  now  a  Senior  Research  Engineer  at  Montreal-based  Jabez  Technologies 
Inc.,  is  to  be  thanked  for  the  excellent  additional  drawings  required  by  the  Third 
Edition,  besides  some  coding,  while  he  was  a  Ph.D.  candidate  at  McGill  University. 
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IX 


Dr.  Stephane  Caro,  currently  a  researcher  at  France’s  Ecole  Centrale  de  Nantes, 
contributed  with  Matlab  coding  while  working  at  McGill  University’s  Robotic 
Mechanical  Systems  Laboratory  as  a  postdoctoral  fellow. 

Montreal,  QC,  Canada  Jorge  Angeles 
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No  todos  los  pensamientos  son  algontmicos. 

— Mario  Bunge ^ 


The  beginnings  of  modem  robotics  can  be  traced  back  to  the  late  1960s  with 
the  advent  of  the  microprocessor,  which  made  possible  the  computer  control  of  a 
multiaxial  manipulator.  Since  those  days,  robotics  has  evolved  from  a  technology 
developed  around  this  class  of  manipulators  for  the  replaying  of  a  preprogrammed 
task  to  a  multidiscipline  encompassing  many  branches  of  science  and  engineering. 
Research  areas  such  as  computer  vision,  artificial  intelligence,  and  speech  recogni¬ 
tion  play  key  roles  in  the  development  and  implementation  of  robotics;  these  are, 
in  turn,  multidisciplines  supported  by  computer  science,  electronics,  and  control,  at 
their  very  foundations.  Thus  we  see  that  robotics  covers  a  rather  broad  spectrum  of 
knowledge,  the  scope  of  this  book  being  only  a  narrow  band  of  this  spectrum,  as 
outlined  below. 

Contemporary  robotics  aims  at  the  design,  control,  and  implementation  of 
systems  capable  of  performing  a  task  defined  at  a  high  level,  in  a  language 
resembling  those  used  by  humans  to  communicate  among  themselves.  Moreover, 
robotic  systems  can  take  on  forms  of  all  kinds,  ranging  from  the  most  intangible, 
such  as  interpreting  images  collected  by  a  space  sound,  to  the  most  concrete,  such 
as  cutting  tissue  in  a  surgical  operation.  We  can,  therefore,  notice  that  motion  is  not 
essential  to  a  robotic  system,  for  this  system  is  meant  to  replace  humans  in  many 
of  their  activities,  moving  being  but  one  of  them.  However,  since  robots  evolved 
from  early  programmable  manipulators,  one  tends  to  identify  robots  with  motion 


^Not  all  thinking  processes  are  algorithmic — translation  of  the  author — personal  communication 
during  the  Symposium  on  the  Brain-Mind  Problem.  A  Tribute  to  Professor  Mario  Bunge  on  His 
75th  Birthday,  Montreal,  September  30,  1994. 
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and  manipulation.  Certainly,  robots  may  rely  on  a  mechanical  system  to  perform 
their  intended  tasks.  When  this  is  the  case,  we  can  speak  of  robotic  mechanical 
systems,  which  are  the  subject  of  this  book.  These  tasks,  in  turn,  can  be  of  a  most 
varied  nature,  mainly  involving  motions  such  as  manipulation,  but  they  can  also 
involve  locomotion.  Moreover,  manipulation  can  be  as  simple  as  displacing  objects 
from  a  belt  conveyor  to  a  magazine.  On  the  other  hand,  manipulation  can  also  be 
as  complex  as  displacing  these  objects  while  observing  constraints  on  both  motion 
and  force,  e.g.,  when  cutting  live  tissue  of  vital  organs.  We  can,  thus,  distinguish 
between  plain  manipulation  and  dextrous  manipulation.  Furthermore,  manipulation 
can  involve  locomotion  as  well. 

The  task  of  a  robotic  mechanical  system  is,  hence,  intimately  related  to  motion 
control,  which  warrants  a  detailed  study  of  mechanical  systems  as  elements  of  a 
robotic  system.  The  aim  of  this  book  can,  therefore,  be  stated  as  establishing  the 
foundations  on  which  the  design,  control,  and  implementation  of  robotic  mechanical 
systems  are  based. 

The  book  evolved  from  sets  of  lecture  notes  developed  at  McGill  University  over 
the  last  12  years,  while  I  was  teaching  a  two- semester  sequence  of  courses  on  robotic 
mechanical  systems.  For  this  reason,  the  book  comprises  two  parts — an  introductory 
and  an  intermediate  part  on  robotic  mechanical  systems.  Advanced  topics,  such 
as  redundant  manipulators,  manipulators  with  flexible  links  and  joints,  and  force 
control,  are  omitted.  The  feedback  control  of  robotic  mechanical  systems  is  also 
omitted,  although  the  book  refers  the  reader,  when  appropriate,  to  the  specialized 
literature.  An  aim  of  the  book  is  to  serve  as  a  textbook  in  a  1-year  robotics  course; 
another  aim  is  to  serve  as  a  reference  to  the  practicing  engineer. 

The  book  assumes  some  familiarity  with  the  mathematics  taught  in  any  engineer¬ 
ing  or  science  curriculum  in  the  first  2  years  of  college.  Familiarity  with  elementary 
mechanics  is  helpful,  but  not  essential,  for  the  elements  of  this  science  needed  to 
understand  the  mechanics  of  robotic  systems  are  covered  in  the  first  three  chapters, 
thereby  making  the  book  self-contained.  These  three  chapters,  moreover,  are  meant 
to  introduce  the  reader  to  the  notation  and  the  basics  of  mathematics  and  rigid-body 
mechanics  needed  in  the  study  of  the  systems  at  hand.  The  material  covered  in  the 
same  chapters  can  thus  serve  as  reading  material  for  a  course  on  the  mathematics 
of  robotics,  intended  for  sophomore  students  of  science  and  engineering,  prior  to  a 
more  formal  course  on  robotics. 

The  first  chapter  is  intended  to  give  the  reader  an  overview  of  the  subject 
matter  and  to  highlight  the  major  issues  in  the  realm  of  robotic  mechanical 
systems.  Chapter  2  is  devoted  to  notation,  nomenclature,  and  the  basics  of  linear 
transformations  to  understand  best  the  essence  of  rigid-body  kinematics,  an  area 
that  is  covered  in  great  detail  throughout  the  book.  A  unique  feature  of  this  chapter 
is  the  discussion  of  the  hand-eye  calibration  problem:  Many  a  paper  has  been 
written  in  an  attempt  to  solve  this  fundamental  problem,  always  leading  to  a 
cumbersome  solution  that  invokes  nonlinear-equation  solving,  a  task  that  invariably 
calls  for  an  iterative  procedure;  moreover,  within  each  iteration,  a  singular- value 
decomposition,  itself  iterative  as  well,  is  required.  In  Chap.  2,  a  novel  approach  is 
introduced,  which  resorts  to  invariant  properties  of  rotations  and  leads  to  a  direct 
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solution,  involving  straightforward  matrix  and  vector  multiplications.  Chapter  3 
reviews,  in  turn,  the  basic  theorems  of  rigid-body  kinetostatics  and  dynamics. 
The  viewpoint  here  represents  a  major  departure  from  most  existing  books  on 
robotic  manipulators:  proper  orthogonal  matrices  can  be  regarded  as  coordinate 
transformations  indeed,  but  they  can  also  be  regarded  as  representations,  once 
a  coordinate  frame  has  been  selected,  of  rigid-body  rotations.  I  adopt  the  latter 
viewpoint,  and  hence  fundamental  concepts  are  explained  in  terms  of  their  invariant 
properties,  i.e.,  properties  that  are  independent  of  the  coordinate  frame  adopted. 
Hence,  matrices  are  used  first  and  foremost  to  represent  the  physical  motions 
undergone  by  rigid  bodies  and  systems  thereof;  they  are  to  be  interpreted  as  such 
when  studying  the  basics  of  rigid-body  mechanics  in  this  chapter.  Chapter  4  is  the 
first  chapter  entirely  devoted  to  robotic  mechanical  systems,  properly  speaking. 
This  chapter  covers  extensively  the  kinematics  of  robotic  manipulators  of  the 
serial  type.  However,  as  far  as  displacement  analysis  is  concerned,  the  chapter 
limits  itself  to  the  simplest  robotic  manipulators,  namely,  those  with  a  decoupled 
architecture,  i.e.,  those  that  can  be  decomposed  into  a  regional  architecture  for  the 
positioning  of  one  point  of  their  end-effector  (EE),  and  a  local  architecture  for  the 
orientation  of  their  EE.  In  this  chapter,  the  notation  of  Denavit  and  Hartenberg 
is  introduced  and  applied  consistently  throughout  the  book.  Jacobian  matrices, 
workspaces,  singularities,  and  kinetostatic  performance  indices  are  concepts  studied 
in  this  chapter.  A  novel  algorithm  is  included  for  the  determination  of  the  workspace 
boundary  of  positioning  manipulators.  Eurthermore,  Chap.  5  is  devoted  to  the  topic 
of  trajectory  planning,  while  limiting  its  scope  to  problems  suitable  to  a  first  course 
on  robotics;  this  chapter  thus  focuses  on  pick-and-place  operations.  Chapter  6, 
moreover,  introduces  the  dynamics  of  robotic  manipulators  of  the  serial  type, 
while  discussing  extensively  the  recursive  Newton-Euler  algorithm  and  laying  the 
foundations  of  multibody  dynamics,  with  an  introduction  to  the  Euler-Lagrange 
formulation.  The  latter  is  used  to  derive  the  general  algebraic  structure  of  the 
mathematical  models  of  the  systems  under  study,  thus  completing  the  introductory 
part  of  the  book. 

The  intermediate  part  comprises  four  chapters.  Chapter  7  is  devoted  to  the 
increasingly  important  problem  of  determining  the  angular  velocity  and  the  angular 
acceleration  of  a  rigid  body,  when  the  velocity  and  acceleration  of  a  set  of  its  points 
are  known.  Moreover,  given  the  intermediate  level  of  the  chapter,  only  the  theoret¬ 
ical  aspects  of  the  problem  are  studied,  and  hence,  perfect  measurements  of  point 
position,  velocity,  and  acceleration  are  assumed,  thereby  laying  the  foundations  for 
the  study  of  the  same  problems  in  the  presence  of  noisy  measurements.  This  problem 
is  finding  applications  in  the  control  of  parallel  manipulators,  which  is  the  reason 
why  it  is  included  here.  If  time  constraints  so  dictate,  this  chapter  can  be  omitted, 
for  it  is  not  needed  in  the  balance  of  the  book. 

The  formulation  of  the  inverse  kinematics  of  the  most  general  robotic  manip¬ 
ulator  of  the  serial  type,  leading  to  a  univariate  polynomial  of  the  16th  degree, 
not  discussed  in  previous  books  on  robotics,  is  included  in  Chap.  8.  Likewise, 
the  direct  kinematics  of  the  platform  manipulator  popularly  known  as  the  Stewart 
platform,  a.k.a.  the  Stewart-Gough  platform,  leading  to  a  16th-degree  mono  variate 
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polynomial,  is  also  given  due  attention  in  this  chapter.  Moreover,  an  alternative 
approach  to  the  monovariate-polynomial  solution  of  the  two  foregoing  problems, 
that  is  aimed  at  solving  them  semigraphically,  is  introduced  in  this  chapter.  With 
this  approach,  the  underlying  multivariate  algebraic  system  of  equations  is  reduced 
to  a  system  of  two  nonlinear  bivariate  equations  that  are  trigonometric  rather  than 
polynomial.  Each  of  these  two  equations,  then,  leads  to  a  contour  in  the  plane 
of  the  two  variables,  the  desired  solutions  being  found  as  the  coordinates  of  the 
intersections  of  the  two  contours. 

Discussed  in  Chap.  9  is  the  problem  of  trajectory  planning  as  pertaining  to 
continuous  paths,  which  calls  for  some  concepts  of  differential  geometry,  namely, 
the  Frenet-Serret  equations  relating  the  tangent,  normal,  and  binormal  vectors  of 
a  smooth  curve  to  their  rates  of  change  with  respect  to  the  arc  length.  The  chapter 
relies  on  cubic  parametric  splines  for  the  synthesis  of  the  generated  trajectories  in 
joint  space,  starting  from  their  descriptions  in  Cartesian  space.  Finally,  Chap.  10 
completes  the  discussion  initiated  in  Chap.  6,  with  an  outline  of  the  dynamics  of 
parallel  manipulators  and  rolling  robots.  Here,  a  multibody  dynamics  approach  is 
introduced,  as  in  the  foregoing  chapter,  that  eases  the  formulation  of  the  underlying 
mathematical  models. 

Two  appendices  are  included:  Appendix  A  summarizes  a  series  of  facts  from  the 
kinematics  of  rotations,  that  are  available  elsewhere,  with  the  purpose  of  rendering 
the  book  self-contained;  Appendix  B  is  devoted  to  the  numerical  solution  of  over- 
and  underdetermined  linear  algebraic  systems,  its  purpose  being  to  guide  the  reader 
to  the  existing  robust  techniques  for  the  computation  of  least- square  and  minimum- 
norm  solutions.  The  book  concludes  with  a  set  of  problems,  along  with  a  list  of 
references,  for  all  ten  chapters. 


On  Notation 

The  important  issue  of  notation  is  given  due  attention.  In  figuring  out  the  notation,  I 
have  adopted  what  I  call  the  norm.  Under  this  norm,  the  notation  should  be 

1.  Comprehensive, 

2.  Concise,  and 

3.  Consistent. 

Within  this  norm,  I  have  used  boldface  fonts  to  indicate  vectors  and  matrices,  with 
uppercases  reserved  for  matrices  and  lowercases  for  vectors.  In  compliance  with  the 
invariant  approach  adopted  at  the  outset,  I  do  not  regard  vectors  solely  as  arrays,  but 
as  geometric  or  mechanical  objects.  Regarding  such  objects  as  arrays  is  necessary 
only  when  it  is  required  to  perform  operations  with  them  for  a  specific  purpose.  An 
essential  feature  of  vectors  in  a  discussion  is  their  dimension,  which  is  indicated 
with  a  single  number,  as  opposed  to  the  convention  whereby  vectors  are  regarded 
as  matrix  arrays  of  numbers;  in  this  convention,  the  dimension  has  to  be  indicated 
with  two  numbers,  one  for  the  number  of  columns,  and  one  for  the  number  of  rows; 


Preface  to  the  First  Edition 


XV 


in  the  case  of  vectors,  the  latter  is  always  one,  and  hence  need  not  be  mentioned. 
Additionally,  calligraphic  literals  are  reserved  for  sets  of  points  or  of  other  objects. 
Since  variables  are  defined  every  time  that  they  are  introduced,  and  the  same  variable 
is  used  in  the  book  to  denote  different  concepts  in  different  contexts,  a  list  of 
symbols  is  not  included. 


How  to  Use  the  Book 

The  book  can  be  used  as  a  reference  or  as  a  text  for  the  teaching  of  the  mechanics  of 
robots  to  an  audience  that  ranges  from  junior  undergraduates  to  doctoral  students. 
In  an  introductory  course,  the  instructor  may  have  to  make  choices  regarding  what 
material  to  skip,  given  that  the  duration  of  a  regular  semester  does  not  allow  to 
cover  all  that  is  included  in  the  first  six  chapters.  Topics  that  can  be  skipped,  if 
time  so  dictates,  are  the  discussions,  in  Chap.  4,  of  workspaces  and  performance 
indices,  and  the  section  on  simulation  in  Chap.  6.  Under  strict  time  constraints,  the 
whole  Chap.  5  can  be  skipped,  but  then,  the  instructor  will  have  to  refrain  from 
assigning  problems  or  projects  that  include  calculating  the  inverse  dynamics  of  a 
robot  performing  pick-and-place  operations.  None  of  these  has  been  included  in 
Sect.  6  of  the  Exercises. 

If  sections  of  Chaps.  4  and  5  have  been  omitted  in  a  first  course,  it  is  highly 
advisable  to  include  them  in  a  second  course,  prior  to  discussing  the  chapters 
included  in  the  intermediate  part  of  the  book. 
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Chapter  1 

An  Overview  of  Robotic  Mechanical  Systems 


1.1  Introduction 

In  defining  the  scope  of  our  subject,  we  have  to  establish  the  genealogy  of  robotic 
mechanical  systems.  These  are,  obviously,  a  subclass  of  the  much  broader  class  of 
mechanical  systems.  Mechanical  systems,  in  turn,  constitute  a  subset  of  the  more 
general  concept  of  dynamic  systems.  In  the  end,  we  must  have  an  idea  of  what,  in 
general,  a  system  is. 

The  Concise  Oxford  Dictionary  defines  system  as  a  “complex  whole,  set  of 
connected  things  or  parts,  organized  body  of  material  or  immaterial  things,”  whereas 
the  Random  House  College  Dictionary  defines  the  same  as  “an  assemblage  or 
combination  of  things  or  parts  forming  a  complex  or  unitary  whole.”  Le  Petit 
Robert,  in  turn,  defines  system  as  ''Ensemble  possedant  une  structure,  constituant 
un  tout  organiquef  which  can  be  loosely  translated  as  “A  structured  assemblage 
constituting  an  organic  whole.”  In  the  foregoing  definitions,  we  note  that  the 
underlying  idea  is  that  of  a  set  of  elements  interacting  as  a  whole. 

On  the  other  hand,  a  dynamic  system  is  a  subset  of  the  set  of  systems.  For  our 
purposes,  we  can  dispense  with  a  rigorous  definition  of  this  concept.  Suffice  it  to 
say  that,  to  qualify  as  dynamic,  a  system  should  be  endowed  with  three  elements, 
namely,  a  state,  an  input,  and  an  output,  in  addition  to  a  rule  of  transition  from  one 
current  state  to  sl  future  one.  Moreover,  the  state  is  sl  functional  of  the  input  and  a 
function  of  a  previous  state.  In  this  concept,  then,  the  idea  of  order  is  important,  and 
can  be  taken  into  account  by  properly  associating  each  state  value  with  time.  The 
state  at  every  instant  is  a  functional,  as  opposed  to  a  function,  of  the  input,  which  is 
characteristic  of  dynamic  systems.  This  means  that  the  state  of  a  dynamic  system  at 
a  certain  instant  is  determined  not  only  by  the  value  of  the  input  at  that  instant,  but 
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also  by  the  past  history  of  the  input — besides,  of  course,  its  initial  state.  By  virtue 
of  this  property,  dynamic  systems  are  said  to  have  memory. 

On  the  contrary,  systems  whose  state  at  a  given  instant  is  only  sl  function  of  the 
input  at  the  current  time  are  static,  and  said  to  have  no  memory.  Additionally,  since 
the  state  of  a  dynamic  system  is  a  result  of  all  the  past  history  of  the  input,  the 
future  values  of  this  having  no  influence  on  the  state,  dynamic  systems  are  said  to 
be  nonanticipative  or  causal.  By  the  same  token,  systems  whose  state  is  the  result  of 
future  values  of  the  input  are  said  to  be  anticipative  or  noncausal.  In  fact,  we  need 
not  worry  about  the  latter,  and  hence,  all  systems  we  will  study  will  be  assumed  to 
be  causal. 

Obviously,  a  mechanical  system  is  a  system  composed  of  mechanical  elements. 
If  this  system  complies  with  the  definition  of  dynamic  system,  then  we  end  up  with 
a  dynamic  mechanical  system.  For  brevity,  we  will  refer  to  such  systems  as  mechan¬ 
ical  systems,  the  dynamic  property  being  implicit  throughout  the  book.  Mechanical 
systems  of  this  type  are  those  that  occur  whenever  the  inertia  of  their  elements  is 
accounted  for.  Static  mechanical  systems  are  those  in  which  inertia  is  neglected. 
Moreover,  the  elements  constituting  a  mechanical  system  are  rigid  and  deformable 
solids,  compressible  and  incompressible  fluids,  and  inviscid  and  viscous  fluids. 

From  the  foregoing  discussion,  then,  it  is  apparent  that  mechanical  systems  can 
be  constituted  either  by  lumped-parameter  or  by  distributed-parameter  elements. 
The  former  reduce  to  particles;  rigid  bodies;  massless,  conservative  springs;  and 
massless,  nonconservative  dashpots.  The  latter  appear  whenever  bodies  are  modeled 
as  continuous  media.  In  this  book,  we  will  focus  on  lumped-parameter  mechanical 
systems.  In  mechanical  systems,  the  driving  forces  and  moments  exerted  by  the 
actuators  and  the  environment  play  the  role  of  the  input,  the  set  of  signals  picked  up 
by  the  sensors  that  of  the  output.  Finally,  the  rules  of  transition  are  dictated  by  the 
laws  of  nature,  especially  from  mechanics,  electromagnetics  and  biology. 

Furthermore,  a  mechanical  system  can  be  either  natural  or  engineered}  the 
latter  being  the  subject  of  our  study.  Engineered  mechanical  systems  can  be  either 
controlled  or  uncontrolled.  Most  engineering  systems  are  controlled  mechanical 
systems,  and  hence,  we  will  focus  on  these.  Moreover,  a  controlled  mechanical 
system  may  be  robotic  or  nonrobotic.  The  latter  are  systems  supplied  with  primitive 
controllers,  mostly  analog,  such  as  thermostats,  servovalves,  etc.  Robotic  mechan¬ 
ical  systems,  in  turn,  can  be  programmable,  such  as  most  current  industrial  robots, 
or  intelligent,  as  discussed  below.  Programmable  mechanical  systems  obey  motion 
commands  either  stored  in  a  memory  device  or  generated  on-line.  In  either  case, 
they  need  sensors,  such  as  joint  encoders,  accelerometers,  and  dynamometers. 

Intelligent  robots  or,  more  broadly  speaking,  intelligent  machines,  are  yet  to 
be  demonstrated,  but  have  become  the  focus  of  intensive  research.  If  intelligent 


Tn  the  previous  editions  we  had  used  the  term  “man-made”  instead.  To  avoid  a  gender-biased 
terminology,  we  could  have  used  “artificial,”  but  this  term,  while  meaning  “human-made,”  also  has 
a  negative  connotation:  “lacking  in  natural  or  spontaneous  quality.” 
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machines  are  ever  feasible,  they  will  depend  highly  on  a  sophisticated  sensory 
system  and  the  associated  hardware  and  software  for  the  processing  of  the  infor¬ 
mation  supplied  by  the  sensors.  The  processed  information  would  then  be  supplied 
to  the  actuators  in  charge  of  producing  the  desired  robot  motion.  Contrary  to  pro¬ 
grammable  robots,  whose  operation  is  limited  to  structured  environments,  intelligent 
machines  should  be  capable  of  reacting  to  unpredictable  changes  in  an  unstructured 
environment.  Thus,  intelligent  machines  should  be  supplied  with  decision-making 
capabilities  aimed  at  mimicking  the  natural  decision-making  process  of  living 
organisms.  This  is  the  reason  why  such  systems  are  termed  intelligent  in  the  first 
place.  Thus,  intelligent  machines  are  expected  to  perceive  their  environment  and 
draw  conclusions  based  on  this  perception.  What  is  supposed  to  make  these  systems 
intelligent  is  their  capability  of  perceiving,  which  involves  a  certain  element  of 
subjectivity.  By  far,  the  most  complex  of  perception  tasks,  both  in  humans  and 
machines,  is  visual  (Levine  1985;  Horn  1986). 

In  summary,  then,  an  intelligent  machine  is  expected  to  (a)  perceive  the  environ¬ 
ment;  (b)  reason  about  this  perception;  (c)  make  decisions  based  on  this  reasoning; 
and  (d)  act  according  to  a  plan  specified  at  a  very  high  level.  What  the  latter  means  is 
that  the  motions  undergone  by  the  machine  are  decided  upon  based  on  instructions 
similar  to  those  given  to  a  human  being,  like  bring  me  a  glass  of  water  without 
spilling  the  water. 

Whether  intelligent  machines  with  all  the  above  features  will  be  1  day  possible 
or  not  is  still  a  subject  of  discussion,  sometimes  at  a  philosophical  level.  Penrose 
(1994)  wrote  a  refutal  to  the  claim  that  intelligent  machines  are  possible. 

A  genealogy  of  mechanical  systems,  including  robotic  ones,  is  given  in  Fig.  1.1. 
In  that  figure,  we  have  drawn  a  dashed  line  between  mechanical  systems  and 
other  systems,  both  engineered  and  natural.  This  line  is  intended  to  emphasize 
the  interaction  of  mechanical  systems  with  electrical,  thermal,  and  other  systems, 
including  the  human  system,  which  is  present  in  telemanipulators,  to  be  discussed 
below. 


1.2  The  General  Architecture  of  Robotic  Mechanical 
Systems 

From  Sect.  1.1,  then,  a  robotic  mechanical  system  is  composed  of  a  few  subsystems, 
namely,  (a)  a  mechanical  subsystem  composed  in  turn  of  both  rigid  and  deformable 
bodies,  although  the  systems  we  will  study  here  are  composed  only  of  the  former; 
(b)  a  sensing  subsystem;  (c)  an  actuation  subsystem;  (d)  a  controller;  and  (e)  an 
information-processing  subsystem.  Additionally,  these  subsystems  communicate 
among  themselves  via  interfaces,  whose  function  consists  basically  of  decoding 
the  transmitted  information  from  one  medium  to  another.  Figure  1.2  illustrates  the 
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Fig.  1.1  A  genealogy  of  robotic  mechanical  systems 


task 


description 


SJV:  synthesized  joint  variables  (angles  and  torques) 

P&FS:  position  and  force  signals 

C&JS:  Cartesian  and  joint  signals 

AJV:  actual  joint  variables  (angles  and  torques) 

Fig.  1.2  General  architecture  of  a  robotic  mechanical  system 
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general  architecture^  of  a  typical  robotic  mechanical  system.  The  input  here  is  a 
prescribed  task,  which  is  defined  either  on  the  spot  or  off-line.  The  former  case 
is  essential  for  a  machine  to  be  called  intelligent,  while  the  latter  is  present  in 
programmable  machines.  Thus,  tasks  would  be  described  to  intelligent  machines  by 
a  software  system  based  on  techniques  of  artificial  intelligence  (AI).  This  system 
would  replace  the  human  being  in  the  decision-making  process.  Programmable 
robots  require  human  intervention  either  for  the  coding  of  preprogrammed  tasks 
at  a  very  low  level  or  for  telemanipulation.  A  “very  low  level  of  programming” 
means  that  the  motions  of  the  machine  are  specified  as  a  sequence  of  either  joint 
motions  or  Cartesian  coordinates  associated  with  landmark  points  of  that  specific 
body  performing  the  task  at  hand.  The  output  of  a  robotic  mechanical  system  is  the 
actual  task,  which  is  monitored  by  the  sensors.  The  sensors,  in  turn,  transmit  task 
information  in  the  form  of  feedback  signals,  to  be  compared  with  the  prescribed 
task.  The  errors  between  the  prescribed  and  the  actual  task  are  then  fed  back  into  the 
controller,  which  further  synthesizes  the  necessary  corrective  signals.  These  are,  in 
turn,  fed  back  into  the  actuators,  which  then  drive  the  mechanical  system  through  the 
required  task,  thereby  closing  the  loop.  The  problem  of  robot  control  has  received 
extensive  attention  in  the  literature,  and  will  not  be  pursued  here.  The  interested 
reader  is  referred  to  the  excellent  works  on  the  subject,  e.g.,  those  of  Samson  et  al. 
(1991),  Khalil  and  Dombre  (2002);  and  Spong  et  al.  (2006).  Of  special  relevance  to 
robot  control  is  the  subject  of  nonlinear  control  at  large,  a  pioneer  here  being  Isidori 
(1989). 

Robotic  mechanical  systems  with  a  human  being  in  their  control  loop  are 
called  telemanipulators.  Thus,  a  telemanipulator  is  a  robotic  mechanical  system  in 
which  the  task  is  controlled  by  a  human,  possibly  aided  by  sophisticated  sensors 
and  display  units.  The  human  operator  replaces  the  ROBOT  MODEL  block  in  the 
diagram  of  Fig.  1.2,  produces  the  task  description,  becomes  a  part  of  the  sensory 
system,  and  plays  a  major  role  in  the  INFORMATION  PROCESSING  UNIT 
block.  Based  on  the  information  displayed,  the  operator  makes  decisions  about 
corrections  in  order  to  accomplish  the  prescribed  task.  Shown  in  Fig.  1.3  is  a 
telemanipulator  designed  for  space  applications,  namely,  the  Canadarml,  along 
with  DEXTRE,  the  Special-Purpose  Dextrous  Manipulator  (SPDM),  both  mounted 
on  the  Mobile  Servicing  System  (MSS),  a  module  of  the  International  Space  Station. 
Moreover,  a  detailed  view  of  DEXTRE  is  shown  in  Fig.  1.4.  In  the  manipulators  of 
these  two  figures,  the  human  operator  is  an  astronaut  who  commands  and  monitors 
the  motions  of  the  robot  from  inside  the  EVA  (extravehicular  activity)  workstation. 
The  number  of  controlled  axes  of  each  of  these  manipulators  being  larger  than 


^In  Chap.  4  we  introduce  the  concept  of  robotic  architecture,  to  indicate  the  geometry  of  the 
underlying  mechanical  system.  We  refer  here  to  the  “general  architecture”  of  the  whole  robotic 
system,  to  distinguish  between  the  two  concepts. 
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Fig.  1.3  Canadarm2  and 
DEXTRE  (courtesy  of  the 
Canadian  Space  Agency) 


1  An  Overview  of  Robotic  Mechanical  Systems 


Fig.  1.4  DEXTRE,  the 
special-purpose  dextrous 
manipulator  (courtesy  of  the 
Canadian  Space  Agency) 


six,  both  are  termed  redundant.  The  challenge  here  is  that  the  mapping  from  task 
coordinates  to  joint  motions  is  not  unique,  and  hence,  among  the  infinitely  many 
joint  trajectories  that  the  operator  has  at  his  or  her  disposal  for  a  given  task,  an 
on-board  processor  must  evaluate  the  best  one  according  to  a  performance  criterion. 
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1.2.1  Types  of  Robots  by  Function 

When  the  first  edition  was  written,  in  the  early  nineties,  the  classification  of 
robots  was  rather  straightforward,  for  there  were  mainly  two  kinds:  serial  and 
parallel.  Nowadays  a  robot  classification  is  a  daunting  task,  by  virtue  of  the  intense 
activity  displayed  in  the  areas  of  robotics  research,  robot  design,  innovation  and 
applications.  For  example,  a  look  at  the  proceedings  of  a  recent  edition  of  the 
IEEE  International  Conference  on  Robotics  and  Automation  will  reveal  a  vast 
spectrum  of  robots  currently  working  on  the  shopfloor,  in  the  operating  room,  in 
rehabilitation  centers,  and  even  at  home.  In  attempting  a  classification  of  robots,  the 
most  comprehensive  criterion  would  be  by  function.  We  thus  have  a  tentative,  but 
by  no  means  comprehensive,  classification: 

•  Manipulators:  robotic  arms  and  hands; 

•  motion  generators:  flight  simulators;  SCARA  (Selective-Compliance  Assembly 
Robot  Arm);  and  moving  platforms  at  large; 

•  locomotors,  a.k.a.  mobile  robots:  legged  and  wheeled  robots,  including  rovers; 

•  swimming  robots;  and 

•  flying  robots. 

We  expand  below  on  these  robot  types. 


1.2.2  Types  of  Robots  by  Size 

The  most  common  type  of  robots  under  this  criterion  is  macro-robots,  or  those 
whose  dimensions  are  measured  in  meter.  These  are  robots  with  a  reach  of 
typically  a  couple  of  meters.  Shown  in  Fig.  1.5  is  a  heavy-duty  robot,  IRB-7600, 
manufactured  by  ABB  Robotics,  with  a  reach  of  2.800  m  and  a  load-carrying 
capacity  of  3,332  N.  This  robot  finds  applications  mainly  in  the  manipulation  of 
heavy  parts  in  the  automobile  industry. 

Micro-robots  bear  dimensions  allowing  them  a  reach  of  a  fraction  of 
a  mm.  For  example,  the  robot  reported  by  Sun  et  al.  (2005)  for  MEMS 
(micro-electromechanical  systems)  assembly,  features  a  maximum  reach  of  100  p.m 
in  each  of  two  orthogonal  directions  and  one  of  50  p.m  in  a  direction  orthogonal  to 
these  two. 


1.2.3  Types  of  Robots  by  Application 

Robot  applications  have  widespread  as  much  as  robot  architectures.  Current 
applications  span  the  classical  industrial  robots  for  arc-welding,  for  example, 
on  to  material-handling,  surveillance,  surgical  operations,  rehabilitation  and 
entertainment. 
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Fig.  1.5  The  IRB-7600,  a 
heavy-duty  robotic  arm  with  a 
serial  architecture  (courtesy 
of  ABB  robotics) 


1.3  Manipulators 

Of  all  robotic  mechanical  systems,  manipulators  deserve  special  attention  for 
various  reasons.  One  is  that,  in  their  simplest  form,  as  robotic  arms,  they  occur  most 
frequently  in  industry.  Another  is  that  the  architecture  of  robotic  arms  constitutes  the 
simplest  of  all  robotic  architectures,  and  hence,  appear  as  constituents  of  other,  more 
complex  robotic  mechanical  systems,  as  will  become  apparent  in  later  chapters. 
A  manipulator,  in  general,  is  a  mechanical  system  aimed  at  object  manipulation. 
Manipulating,  in  turn,  means  to  move  something  with  one’s  hands,  as  the  word 
derives  from  the  Latin  manus,  meaning  hand.  The  basic  idea  behind  the  foregoing 
concept  is  that  hands  are  among  the  organs  that  the  human  brain  can  control 
mechanically  with  the  highest  accuracy,  as  the  work  of  an  artist  like  Picasso,  of 
an  accomplished  guitar  player,  or  of  a  surgeon  can  attest. 

A  manipulator  is  thus  any  device  that  helps  a  human  operator  perform  a 
manipulating  task.  Although  manipulators  have  existed  ever  since  man  created  the 
first  tool,  only  very  recently,  namely,  by  the  end  of  World  War  II,  have  manipulators 
developed  to  the  extent  that  they  are  now  capable  of  actually  mimicking  motions  of 
the  human  arm,  and  of  the  human  hand,  for  that  matter.  In  fact,  during  WWII,  the 
need  arose  for  manipulating  probe  tubes  containing  radioactive  substances.  This  led 
to  the  first  six-degree-of-freedom  (six-dof)  manipulators. 

Shortly  thereafter,  the  need  for  manufacturing  workpieces  with  high  accuracy 
arose  in  the  aircraft  industry,  which  led  to  the  first  numerically-controlled  (NC) 
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machine  tools.  The  synthesis  of  the  six-DOF  manipulator  and  the  NC  machine 
tool  produced  what  became  the  robotic  manipulator.  Thus,  the  essential  difference 
between  the  early  manipulator  and  the  evolved  robotic  manipulator  is  the  “robotic” 
qualifier,  which  came  into  the  picture  in  the  late  sixties.  A  robotic  manipulator  is 
to  be  distinguished  from  the  early  manipulator  by  its  capability  of  lending  itself  to 
computer  control.  While  the  early  manipulator  needed  the  presence  of  a  human  in 
the  loop,  to  have  a  master  manipulator  perform  a  gesture,  the  robotic  manipulator 
can  be  programmed  once  and  for  all  to  repeat  the  same  task  forever.  Programmable 
manipulators  owe  their  existence  to  the  microprocessor.  Indeed,  the  microprocessor, 
introduced  in  1976  by  Intel,  allowed  a  human  master  to  teach  the  manipulator 
by  actually  driving  the  manipulator  itself,  or  a  replica  thereof,  through  a  desired 
task,  while  recording  all  motions  undergone  by  the  master.  Thus,  the  manipulator 
would  later  repeat  the  identical  task  by  mere  playback.  However,  the  capabilities 
of  industrial  robots  are  fully  exploited  only  if  the  manipulator  is  programmed 
with  software,  rather  than  actually  driving  it  through  its  task  trajectory,  which 
many  a  time,  e.g.,  in  car-body  spot-welding,  requires  separating  the  robot  from  the 
production  line  for  more  than  a  week.  One  of  the  objectives  of  this  book  is  to  develop 
tools  for  the  programming  of  robotic  manipulators. 

Nevertheless,  the  capabilities  offered  by  robotic  mechanical  systems  go  well 
beyond  the  mere  playback  of  preprogrammed  tasks.  Current  research  aims  to 
providing  robotic  systems  with  software  and  hardware  that  will  allow  them  to  make 
decisions  on  the  spot  and  learn  while  performing  a  task.  The  implementation  of  such 
systems  calls  for  task-planning  techniques  that  fall  beyond  the  scope  of  this  book 
and,  hence,  will  not  be  treated  here.  For  a  glimpse  of  such  techniques,  the  reader  is 
referred  to  the  work  of  Latombe  (1991)  and  the  references  therein. 


1.3.1  Robotic  Arms 

Robotic  manipulators  first  appeared  as  mechanical  systems  resembling  the  human 
arm.  Robotic  arms  are  thus  constituted  by  a  mechanical  system  consisting  of 
structurally  robust  links  coupled  by  either  rotational  or  translating  joints,  the  former 
being  called  revolutes,  the  latter  prismatic  joints.  Moreover,  these  structures  are  a 
concatenation  of  links,  thereby  forming  an  open  kinematic  chain,  with  each  link 
coupled  to  a  predecessor  and  a  successor,  except  for  the  two  end  links,  which  are 
coupled  only  to  either  a  predecessor  or  to  a  successor,  but  not  to  both.  The  robot 
displayed  in  Fig.  1.5  is  an  example  of  a  robotic  arm  with  strong  links. 

Because  of  the  serial  nature  of  the  coupling  of  links  in  this  type  of  manipulator, 
even  if  they  are  supplied  with  structurally  robust  links,  their  load-carrying  capacity 
and  their  stiffness  is  too  low  when  compared  with  the  other  multiaxis  machines,  such 
as  NC  machine  tools.  Obviously,  a  low  stiffness  implies  a  low  positioning  accuracy. 
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1.3.2  Robotic  Hands 

Besides  the  hand,  other  mechanical  subsystems  constituting  the  human  manipu¬ 
lation  system  are  the  arm  and  the  forearm.  Moreover,  the  shoulder,  coupling  the 
arm  with  the  torso,  can  be  regarded  as  a  spherical  joint,  i.e.,  the  concatenation  of 
three  revolute  joints  with  intersecting  axes.  Furthermore,  the  arm  and  the  forearm 
are  coupled  via  the  elbow,  with  the  forearm  and  the  hand  finally  being  coupled 
by  the  wrist.  Frequently,  the  wrist  is  modeled  as  a  spherical  joint  as  well,  while 
the  elbow  is  modeled  as  a  simple  revolute  joint.  Robotic  mechanical  systems 
mimicking  the  motions  of  the  arm  and  the  forearm  constitute  the  manipulators 
discussed  above.  Here  we  outline  more  sophisticated  manipulation  systems  that  aim 
to  produce  the  motions  of  the  human  hand,  i.e.,  robotic  hands.  These  systems  are 
designed  to  perform  manipulation  tasks,  a  distinction  being  made  between  simple 
manipulation  and  dextrous  manipulation.  What  the  former  means  is  the  simplest 
form,  in  which  the  fingers  play  a  minor  role,  namely,  by  serving  as  simple  static 
structures  that  keep  an  object  rigidly  attached  to  the  palm  of  the  hand — when  the 
palm  is  regarded  as  a  rigid  body.  As  opposed  to  simple  manipulation,  dextrous 
manipulation  involves  a  controlled  motion  of  the  grasped  object  with  respect  to 
the  palm.  Simple  manipulation  can  be  achieved  with  the  aid  of  a  manipulator  and 
a  gripper,  and  need  not  be  further  discussed  here.  The  discussion  here  is  about 
dextrous  manipulation. 

In  dextrous  manipulation,  the  grasped  object  is  required  to  move  with  respect 
to  the  palm  of  the  grasping  hand.  This  kind  of  manipulation  appears  in  performing 
tasks  that  require  high  levels  of  accuracy,  like  handwriting  or  cutting  tissue  with  a 
scalpel.  Usually,  grasping  hands  are  multifingered,  although  some  grasping  devices 
exist  that  are  constituted  by  a  simple,  open,  highly  redundant  kinematic  chain  (Petti- 
nato  and  Stephanou  1989).  The  kinematics  of  grasping  is  discussed  in  Chap.  10.  The 
basic  kinematic  structure  of  a  multifingered  hand  consists  of  a  palm,  which  plays  the 
role  of  the  base  of  a  simple  manipulator,  and  a  set  of  fingers.  Thus,  kinematically 
speaking,  a  multifingered  hand  has  a  tree  topology,  i.e.,  it  entails  a  common  rigid 
body,  the  palm,  and  a  set  of  jointed  bodies  emanating  from  the  palm.  Upon  grasping 
an  object  with  all  the  fingers,  the  chain  becomes  closed,  with  multiple  loops. 
Moreover,  the  architecture  of  the  fingers  is  that  of  a  simple  manipulator,  consisting 
of  a  number — two  to  four — of  revolute-coupled  links  playing  the  role  of  phalanges. 
However,  unlike  manipulators  of  the  serial  type,  whose  joints  are  all  independently 
actuated,  those  of  a  mechanical  finger  are  not  and,  in  many  instances,  are  driven  by 
one  single  master  actuator,  the  remaining  joints  acting  as  slaves.  Many  versions 
of  multifingered  hands  exist:  Stanford/JPL;  Utah/MIT;  TU  Munich;  Karlsruhe; 
Bologna;  Leuven;  Milan;  Belgrade;  and  University  of  Toronto,  among  others.  Of 
these,  the  Utah/MIT  Hand  (Jacobsen  et  al.  1984,  1986)  is  commercially  available. 
This  hand  carries  four  fingers,  one  of  which  is  opposed  to  the  other  three  and 
hence,  plays  the  role  of  the  human  thumb.  Each  finger  consists,  in  turn,  of  four 
phalanges  coupled  by  revolute  joints;  each  of  these  is  driven  by  two  tendons  that 
can  deliver  force  only  when  in  tension,  each  being  actuated  independently.  The  TU 
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Fig.  1.6  Two  instances  of  robotic  hands:  (a)  the  four-fingered  hydraulically  actuated  TU  Munich 
Hand  (courtesy  of  Prof.  F.  Pfeiffer);  and  (b)  Universite  Laval’s,  three-fingered  SARAH  (courtesy 
of  Prof.  C.  Gosselin) 


Munich  Hand,  shown  in  Fig.  1.6a,  is  designed  with  four  identical  fingers  laid  out 
symmetrically  on  a  hand  palm.  This  hand  is  hydraulically  actuated,  and  provided 
with  a  very  high  payload-to-weight  ratio.  Indeed,  each  finger  weighs  only  1.470  N, 
but  can  exert  a  force  of  up  to  30  N.  A  three-fingered  hand  with  12  degrees  of 
freedom  and  six  actuators,  SARAH,  was  designed  at  Universite  Laval’s  Laboratoire 
de  Robotique.  This  hand,  illustrated  in  Fig.  1.6b,  is  twice  as  big  as  the  human  hand, 
weighs  88.2  N,  and  can  hold  a  686-N  load  (Laliberte  et  al.  2002). 

We  outline  below  some  problems  and  research  trends  in  the  area  of  dextrous 
hands.  A  key  issue  here  is  the  programming  of  the  motions  of  the  fingers,  which  is 
a  much  more  complicated  task  than  the  programming  of  a  six-axis  manipulator. 
In  this  regard,  Liu  et  al.  (1989)  introduced  a  task-analysis  approach  intended 
to  program  robotic  hand  motions  at  a  higher  level.  These  researchers  used  a 
heuristic,  knowledge-based  approach.  From  an  analysis  of  the  various  modes  of 
grasping,  they  concluded  that  the  requirements  for  grasping  tasks  are  (a)  stability, 
(b)  manipulability,  (c)  torquability,  and  (d)  radial  rotatability.  Stability  is  defined 
as  a  measure  of  the  tendency  of  an  object  to  return  to  its  original  position 
after  disturbances.  Manipulability,  as  understood  in  this  context,  is  the  ability  to 
impart  motion  to  the  object  while  keeping  the  fingers  in  contact  with  the  object. 
Torquability,  or  tangential  rotatability,  is  the  ability  to  rotate  the  long  axis  of  an 
object — here  the  authors  must  assume  that  the  manipulated  objects  are  convex  and 
can  be  approximated  by  three-axis  ellipsoids,  thereby  distinguishing  between  a  long 
and  a  short  axis — with  a  minimum  force,  for  a  prescribed  amount  of  torque.  Finally, 
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radial  rotatability  is  the  ability  to  rotate  the  grasped  object  about  its  long  axis  with 
minimum  torque  about  the  axis. 

Furthermore,  Allen  et  al.  (1989)  introduced  an  integrated  system  of  both 
hardware  and  software  for  dextrous  manipulation.  The  system  integrates  force  and 
position  sensors  with  control  commands  for  both  the  arm  and  the  hand.  To  demon¬ 
strate  the  effectiveness  of  their  system,  the  authors  implemented  a  task  consisting 
of  removing  a  light  bulb  from  its  socket.  Rus  (1992)  proposed,  in  turn,  a  paradigm 
allowing  the  high-level,  task-oriented  manipulation  control  of  planar  hands. 

While  the  technological  aspects  of  dextrous  manipulation  are  highly  advanced, 
theoretical  aspects  are  still  under  research  in  this  area.  An  extensive  literature  survey, 
with  405  references  on  the  subject  of  manipulation,  was  given  by  Reynaerts  (1995). 
But  that  was  the  state  of  the  art  in  the  early  nineties.  In  the  2005  IEEE  International 
Conference  on  Robotics  and  Automation,  there  were  five  sessions  on  grasping, 
robotic-finger  design,  robotic  hands  and  dextrous  manipulation.  An  interesting 
approach  to  the  programming  of  dextrous  hands,  programming  by  demonstration, 
was  reported  by  Ekvall  and  Krajic  (2005),  under  which  the  robotic  hand  is  taught 
how  to  reproduce  the  grasping  sequences  of  a  human  hand.  The  use  of  vision  as  a 
means  of  grasp-planning  was  also  reported  in  this  conference  (Gockel  et  al.  2005). 


1.4  Motion  Generators 

Under  this  heading  we  include  robotic  systems  designed  to  produce  a  certain  class  of 
motions  for  various  purposes,  ranging  from  manipulation  tasks,  e.g.,  the  positioning 
of  a  camera  for  surveillance,  to  the  orientation  of  a  surgeon’s  scalpel,  on  to  moving 
platforms  for  pilot  training,  as  in  flight  simulators,  or  for  entertainment,  to  give 
people  the  realism  of  an  earthquake  or  a  roller-coaster,  or  simply  of  following  a 
musical  rhythm.  Many  a  motion  generator  is  supplied  with  a  parallel  architecture, 
as  described  below. 


1.4.1  Parallel  Robots 

Parallel  robots  were  originally  proposed  to  cope  with  the  problems  encountered 
with  their  serial  counterparts  (Merlet  2006),  namely,  a  limited  load-carrying  capac¬ 
ity,  low  accuracy,  and  low  stiffness.  This  kind  of  robot  was  thus  introduced  to 
withstand  higher  payloads  with  lighter  links.  In  a  parallel  robot,  we  distinguish 
one  base  platform,  one  moving  platform,  and  various  legs  or  limbs.  Each  leg  is, 
in  turn,  a  kinematic  chain  of  the  serial  type,  whose  end-links  are  the  two  platforms. 
Contrary  to  serial  robots,  all  of  whose  joints  are  actuated,  parallel  robots  are  supplied 
with  unactuated  joints,  which  brings  about  a  substantial  difference  between  the  two 
types.  The  presence  of  unactuated  joints  makes  the  analysis  of  parallel  manipulators, 
in  general,  more  complex  than  that  of  serial  robots. 
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Fig.  1.7  Universite  Laval’s 
Agile  Eye,  a 
three-degree-of-freedom 
spherical  robot  with  a  parallel 
architecture  (courtesy  of  Prof. 
Clement  Gosselin) 


A  paradigm  of  parallel  manipulators  is  the  flight  simulator,  consisting  of  six  legs 
actuated  by  hydraulic  pistons.  The  flight  simulator  with  this  architecture  motivated 
the  early  work,  starting  in  the  late  eighties,  on  parallel  robots.  Recently,  an  explosion 
of  novel  designs  of  parallel  robots  has  occurred,  aimed  at  fast  manipulation  tasks. 
An  example  of  these  robots,  departing  from  the  architecture  of  flight  simulators,  is 
Universite  Laval’s  Agile  Eye,  depicted  in  Fig.  1.7.  This  robot  is  designed  with  one 
fixed  base  and  one  moving  platform,  that  carries  a  small  camera.  Base  and  platform 
are  coupled  by  means  of  three  identical  legs,  each  composed  of  two  links  and  three 
revolute  joints.^  Moreover,  the  axes  of  all  nine  revolutes  intersect  at  one  single  point, 
the  center  of  the  mechanical  system.  For  this  reason,  all  robot  links  move,  with 
respect  to  the  base,  under  pure  rotation,  with  the  robot  center  remaining  fixed.  All 
three  direct-drive  motors  are  mounted  on  the  base,  and  actuate  the  proximal  links  of 
the  legs.  This  robot  can  reportedly  produce  angular  velocities  of  the  camera  as  high 
as  l,000°/s  and  angular  accelerations  of  20,000° /s^. 

Other  parallel  robots  have  been  designed  for  fast  assembly  operations,  e.g.,  the 
Delta  robot  (Clavel  1988),  developed  at  the  Lausanne  Federal  Polytechnic  Institute 
(EPFL).  The  Delta  robot  was  designed  to  produce  pure  translations  of  its  end- 
platform  in  3D  space.  An  instance  of  this  robot,  enhanced  with  a  fourth  joint  of 
vertical  axis,  the  FlexPicker,  is  shown  in  Fig.  1.8.  This  robot  is  designed  with  three 
identical  legs,  hanging  from  the  ceiling,  which  is  the  robot  base.  Each  leg  carries  one 
proximal  link,  coupled  to  the  base  by  a  revolute,  which  is  actuated  by  the  leg-motor. 
Furthermore,  this  link  is  coupled  to  the  end-plate  by  means  of  two  revolutes  and 


^It  can  be  appreciated  in  Fig.  1.7  that  the  proximal  links  are  made  up  of  two  curved  beams,  each 
with  an  axis  in  the  form  of  one-quarter  of  a  circle.  These  two  beams  are  rigidly  fastened,  with  their 
planes  forming  a  90°  dihedral  angle. 
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Fig.  1.8  FlexPicker,  a 
realization  of  EPFL’s  Delta 
Robot  (courtesy  of  ABB 
Robotics) 


one  novel  kinematic  pair,  the  IT -pair,  which  is  nothing  but  a  parallelogram  four-bar 
linkage,  the  IT -pair  being  located  between  the  two  revolutes.  It  is  noteworthy  that  the 
FlexPicker  is  supplied  with  one  additional  actuated  joint,  at  the  interface  between 
the  moving  platform  of  the  original  Delta  Robot  and  the  gripper,  appearing  in  the 
figure  as  a  cylindrical  piece.  This  revolute  is  actuated  from  the  base  by  means  of  a 
transmission  mechanism  stemming  from  the  center  of  the  base  in  the  figure. 

Other  instances  of  parallel  robots  can  be  cited:  Hexa  (Pierrot  et  al.  1991), 
developed  at  Universite  de  Montpellier,  as  a  six-degree-of-freedom  extension  of 
Clavel’s  Delta  Robot;  Star  (Herve  and  Sparacino  1992),  developed  at  Ecole  Centrale 
of  Paris;  the  Truss  arm,  developed  at  the  University  of  Toronto  Institute  of  Aerospace 
Studies  (UTIAS),  shown  in  Fig.  1.9a  (Hughes  et  al.  1991);  INRIAs  main  gauche, 
or  left  hand,  developed  by  Merlet  (2006)^  and  shown  in  Fig.  1.9b,  which  is  used 
as  an  aid  to  another  robot,  possibly  of  the  serial  type,  to  enhance  its  dexterity; 
and  McGill  University’s  parallel  manipulator,  intended  as  a  shoulder  module  for 


"^INRIA  is  France’s  Institut  National  de  Recherche  en  Informatique  et  en  Automatique,  the  left 
hand,  and  other  parallel  robots  having  been  developed  at  INRIAs  center  at  Sophia- Antipolis, 
France. 
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Fig.  1.9  A  sample  of  parallel  manipulators:  (a)  the  UTIAS  Trussarm  (courtesy  of  Prof.  R  C. 
Hughes);  (b)  the  Merlet  left  hand  (courtesy  of  Dr.  J.-P.  Merlet);  and  (c)  the  Hayward  shoulder 
module  (courtesy  of  Prof.  V.  Hayward) 

orientation  tasks  (Hayward  1994),  and  capable  of  three-degree-of-freedom  motions, 
produced  by  four  hydraulic  actuators,  which  gives  the  robot  redundant  actuation — 
Fig.  1.9c. 


1.4,2  SCARA  Systems 

SCARA  is  an  acronym  standing  for  Selective-Compliance  Assembly  Robot  Arm,  as 
coined  by  Hiroshi  Makino  (Makino  and  Furuya  1980),  the  inventor  of  this  new 
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class  of  robots.  The  class  was  proposed  as  a  means  to  provide  motion  capabilities 
to  the  end-effector  that  are  required  by  the  assembly  of  printed-board  circuits  and 
other  electronic  devices  with  a  flat  geometry.  Motions  consist  of  three  independent 
translations  and  one  rotation  about  an  axis  of  fixed  orientation,  usually  vertical. 
These  robots  have  received  special  attention  because  of  their  special  structure, 
offering  an  extremely  high  stiffness  about  two  axes  of  tilting — the  axes  normal  to 
the  axis  of  rotation.  The  first  robots  of  this  kind  appeared  with  a  serial  architecture, 
involving  three  revolutes  and  one  prismatic  joint,  the  latter  being  located  either  at 
the  base  or  at  the  end-effector.  These  robots  have  attained  impressive  performance, 
capable  of  cycle  times  of  500  ms  or  lower,  for  a  standard  pick-and-place  operation 
consisting  of:  (a)  upwards  translation  of  25  mm;  (b)  horizontal  translation  of 
300  mm,  concurrently  rotating  through  an  angle  of  180°;  and  (c)  downwards 
translation  of  25  mm.  The  cycle  is  closed  by  returning  to  the  original  posture 
following  exactly  the  same  displacement  program,  but  in  the  reverse  order. 

Given  the  serial  architecture  of  most  SCARA  systems,  it  appears  that  the  cycle 
times  are  extremely  difficult  to  cut  further  and  the  load-carrying  capacity  is  equally 
difficult  to  increase.  This  state  of  affairs  has  motivated  the  emergence  of  alternative 
architectures,  such  as  parallel  or  hybrid  (serial-parallel).  For  example,  Fanuc’s 
M410iB  and  ABB  Robotics’  FlexPicker,  shown  in  Fig.  1.8,  feature  hybrid  SCARA 
architectures  with  long  reaches,  of  around  3  m  and  payloads  of  above  2000  N.  The 
manufacturers  did  this  by  means  of  parallelogram  linkages  capable  of  transmitting 
torque  and  motion  from  a  common  base,  turning  about  a  vertical  axis,  to  two 
horizontal  revolute  joints,  the  fourth  revolute  having  a  vertical  axis.  Interestingly, 
although  these  robots  are  medium-to-heavy-duty  SCARAs,  the  manufacturers  bill 
them  as  “palletizing  robots,”  with  no  relation  to  SCARAs.  As  a  matter  of  fact, 
SCARAs  can  be  regarded  as  generators  of  the  Schonfiies  displacement  subgroup 
(Bottema  and  Roth  1979;  Herve  1999).  For  this  reason,  SCARA  systems  are 
currently  referred  to  as  Schonfiies -motion  generators. 

In  yet  another  attempt  to  overcome  the  natural  limitations  of  serial  SCARAs, 
parallel  architectures  have  been  proposed:  H4,  a  four-limb  Schonfiies-motion  gen¬ 
erator  developed  at  France’s  Universite  de  Montpellier  (Company  et  al.  2001);  the 
four-limb  robot  driven  with  actuated  prismatic  joints  developed  at  Institut  Frangais 
de  Mecanique  Avancee  (Gogu  2004);  and  the  McGill  SMG  developed  at  McGill 
University  (Al-Widyan  and  Angeles  2004),  that  features  only  two  limbs.  However, 
this  robot  is  overconstrained,  besides  exhibiting  a  rather  limited  rotatability  of  its 
moving  platform.  An  alternative  two-limb  parallel  SCARA  was  proposed  recently 
that  features  an  isostatic  kinematic  chain  (Lee  and  Lee  2010),  thereby  allowing 
for  assembly  even  in  the  presence  of  non-negligible  machining  and  manufacturing 
errors.  The  architecture  of  this  robot  is  illustrated  in  Fig.  1.10,  which  features 
two  cylindrical  pairs — two-degree-of-freedom  (two-dof)  joints  that  produce  rotation 
about  an  axis  and  translation  in  a  direction  parallel  to  the  axis — as  drive  units,  to 
produce  Schonfiies  motions.  The  gripper  is  rigidly  fastened  to  a  rod  with  two  screws 
at  the  ends,  of  different  pitches. 
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Fig.  1.10  An  isostatic 
two-limb  robot  capable  of 
SCARA  motions 


1.5  Locomotors 


Under  locomotors  we  include  all  robots  capable  of  displacing  themselves  on  a 
surface  without  any  attachment  to  the  surface.  Here  we  distinguish  two  kinds  of 
robots,  legged  and  wheeled,  as  outlined  below. 


1.5.1  Legged  Robots 

A  common  architecture  of  walking  machines  is  the  hexapod,  examples  of  which  are 
the  Ohio  State  University  (OSU)  Hexapod  (Klein  et  al.  1983)  and  the  OSU  Adaptive 
Suspension  Vehicle  (ASV)  (Song  and  Waldron  1989).  A  six-legged  walking 
machine  with  a  design  that  mimics  the  locomotion  system  of  the  Carausius  morosus 
(Graham  1972),  also  known  as  the  walking  stick,  was  developed  at  the  Technical 
University  of  Munich  (Pfeiffer  et  al.  1995).  A  prototype  of  this  machine,  known 
as  the  TUM  Hexapod,  is  displayed  in  Fig.  1.11.  The  legs  of  the  TUM  Hexapod 
are  operated  under  neural-network  control,  which  gives  them  a  reflex-like  response 
when  encountering  obstacles:  upon  sensing  an  obstacle,  the  leg  bounces  back  and 
tries  again  to  move  forward,  but  raising  the  foot  to  a  higher  level.  Other  legged  robots 
worth  mentioning  as  pioneers  are  the  Sutherland,  Sprout  and  Associates  Hexapod 
(Sutherland  and  Ullner  1984),  the  Titan  series  of  quadrupeds  (Hirose  et  al.  1985) 
and  the  Odetics  series  of  axially  symmetric  hexapods  (Russell  1983). 
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Fig.  1.11  A  prototype  of  the 
TU  Munich  Hexapod 
(courtesy  of  Prof.  F.  Pfeiffer. 
Reproduced  with  permission 
of  TSI  Enterprises,  Inc.) 


Surveys  of  walking  machines,  of  historical  interest  now,  are  those  of  Todd  (1985) 
and  the  special  issue  of  The  International  Journal  of  Robotics  Research  (Vol.  9, 
No.  2). 

Walking  machines  appear  as  the  sole  means  of  providing  locomotion  in  highly 
unstructured  environments.  In  fact,  the  unique  adaptive  suspension  provided  by 
these  machines  allows  them  to  navigate  on  uneven  terrain.  However,  walking 
machines  cannot  traverse  every  type  of  uneven  terrain,  for  they  are  of  limited 
dimensions.  Hence,  if  terrain  irregularities  such  as  a  crevasse  wider  than  the 
maximum  horizontal  leg  reach  or  a  cliff  of  depth  greater  than  the  maximum  vertical 
leg  reach  are  present,  then  the  machine  is  prevented  from  making  any  progress.  This 
limitation,  however,  can  be  overcome  by  providing  the  machine  with  the  capability 
of  attaching  its  feet  to  the  terrain  in  the  same  way  as  a  mountain  climber  goes 
up  a  cliff.  Moreover,  machine  functionality  is  limited  not  only  by  the  topography 
of  the  terrain,  but  also  by  the  terrain  constitution.  Whereas  hard  rock  poses  no 
serious  problem  to  a  walking  machine,  muddy  terrain  can  hamper  its  operation  to 
the  point  that  it  may  jam  the  machine.  Still,  under  such  adverse  conditions,  walking 
machines  offer  a  better  maneuverability  than  other  vehicles.  Recent  work  at  McGill 
University^  on  legged  locomotion  has  led  to  robots  with  robust  designs  allowing 
them  to  negotiate  mud  and  even  ponds.  A  series  of  hexapods,  under  the  name  RHEX, 
has  been  developed  with  these  features,  as  shown  in  Fig.  1.12.  The  same  robot  is 
shown  in  Fig.  1.13  roaming  a  patterned  floor,  of  about  500  mm  in  length,  to  give  a 
clue  on  its  dimensions. 


Humanoids 

An  important  class  of  legged  robots  is  that  of  humanoids.  Pioneer  work  by 
Vukobratovic  and  Stepanenko  (1972)  has  led  to  modem  bipeds  exhibiting  impres- 


^  Originally  led  by  Prof.  Martin  Buehler. 
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Fig.  1.12  RHEX,  a 
six-legged  robot  (courtesy  of 
G.  Dudek,  McGill  University) 


Fig.  1.13  RHEX  walking  on 
a  patterned  floor,  to  indicate 
its  dimensions  (courtesy  of  G. 
Dudek,  McGill  University) 


sive  performance.  Indeed,  work  initiated  in  1986  at  Honda  led  to  ASIMO,  a  robotic 
mechanical  system  integrating  both  manipulation  and  locomotion  in  one  single  unit. 

Research  in  humanoids  is  quite  intensive  at  the  moment,  with  multiple  sessions 
on  the  subject  during  the  annual  IEEE  International  Conference  on  Robotics 
and  Automation,  including  controls,  motion-planning,  design,  voice-mimicry,  and 
human-robot  interaction. 


1.5.2  Wheeled  Robots  and  Rovers 

Robots  in  this  category  are  systems  evolved  from  earlier  systems  called  automatic 
guided  vehicles,  or  AGVs  for  short.  AGVs  in  their  most  primitive  versions  are  four- 
wheeled,  electrically  powered  vehicles  that  perform  moving  tasks  with  a  certain 
degree  of  autonomy.  However,  these  vehicles  are  usually  limited  to  motions  along 
predefined  tracks  that  are  either  railways  or  magnetic  strips  glued  to  the  ground. 
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The  most  common  rolling  robots  use  conventional  wheels,  i.e.,  wheels  consisting 
basically  of  a  pneumatic  tire  mounted  on  a  hub  that  rotates  about  an  axle  fixed  to  the 
robot  platform.  Thus,  the  operation  of  these  machines  does  not  differ  much  from  that 
of  conventional  terrestrial  vehicles.  An  essential  difference  between  rolling  robots 
and  other  robotic  mechanical  systems  is  the  kinematic  constraints  between  wheel 
and  ground  in  the  former.  These  constraints  are  of  a  type  known  as  nonholonomic, 
as  discussed  in  detail  in  Chap.  12.  Nonholonomic  constraints  are  kinematic  relations 
between  point  velocities  and  angular  velocities  that  cannot  be  integrated  in  the  form 
of  algebraic  relations  between  translational  and  rotational  displacement  variables. 
The  outcome  of  this  lack  of  integrability  leads  to  a  lack  of  a  one-to-one  relationship 
between  Cartesian  variables  and  joint  variables.  In  fact,  while  angular  displacements 
read  by  joint  encoders  of  serial  manipulators  determine  uniquely  the  position  and 
orientation  of  their  end-effector,  the  angular  displacement  of  the  wheels  of  rolling 
machines  do  not  determine  the  position  and  orientation  of  the  vehicle  body.  As 
a  matter  of  fact,  the  control  of  rolling  robots  bears  common  features  with  the 
redundancy-resolution  of  manipulators  of  the  serial  type  at  the  joint-rate  level.  In 
these  manipulators,  the  number  of  actuated  joints  is  greater  than  the  dimension  of 
the  task  space.  As  a  consequence,  the  task  velocity  does  not  determine  the  joint 
rates.  Not  surprisingly,  the  two  types  of  problems  have  been  solved  using  the  same 
tools,  namely,  differential  geometry  and  Lie  algebra  (De  Luca  and  Oriolo  1995). 

As  a  means  to  supply  rolling  robots  with  three-dof  capabilities,  not  found 
in  conventional  terrestrial  vehicles,  omnidirectional  wheels  (ODWs)  have  been 
developed.  Examples  of  ODWs  bear  names  such  as  Mekanum  wheels,  Swedish 
wheels,  donators,  or  others.  ODWs  consist  of  a  hub  with  rollers  on  its  periphery  that 
roll  freely  about  their  axes,  the  latter  being  oriented  at  a  constant  angle  with  respect 
to  the  hub  axis.  Rolling  robots  with  ODWs  are,  thus,  three-dof  vehicles,  and  hence, 
can  translate  freely  in  two  horizontal  directions  and  rotate  independently  about  a 
vertical  axis.  However,  like  their  two-dof  counterparts,  three-dof  rolling  robots  are 
also  nonholonomic  devices,  and  thus,  pose  the  same  problems  for  their  control 
as  the  former.  The  kinematics  and  dynamics  of  robots  with  ODWs  are  studied  in 
Sects.  10.5.2  and  12.5.2,  respectively. 

Further  developments  in  the  technology  of  rolling  robots  have  been  reported 
that  incorporate  alternative  types  of  ODWs.  For  example,  Killough  and  Pin  (1992) 
developed  a  rolling  robot  with  what  they  call  orthogonal  ball  wheels,  consisting 
of  spherical  wheels  that  can  rotate  about  two  mutually  orthogonal  axes.  Borenstein 
(1993)  proposed  a  mobile  robot  with  four  degrees  of  freedom;  these  were  achieved 
with  two  chassis  coupled  by  an  extensible  link,  each  chassis  being  driven  by  two 
actuated  conventional  wheels.  West  and  Asada  (1995),  in  turn,  designed  a  rolling 
robot  with  ball  wheels,  i.e.,  balls  that  act  as  omnidirectional  wheels;  each  ball  is 
mounted  on  a  set  of  rollers,  one  of  which  is  actuated;  hence,  three  such  wheels  are 
necessary  to  fully  control  the  vehicle.  The  unactuated  rollers  serve  two  purposes, 
i.e.,  to  provide  stability  to  the  wheels  and  the  vehicle,  and  to  measure  the  rotation  of 
the  ball,  thereby  detecting  slip. 


1 .5  Locomotors 
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Fig.  1.14  QUASIMORO,  a 
quasiholonomic  mobile  robot 


Mobile  Wheeled  Pendulums 

A  new  class  of  wheeled  robots  has  emerged  since  the  turn  of  the  century.  This  class, 
known  as  mobile  wheeled  pendulums  (MWP),  comprises  two  coaxial  wheels  and  an 
intermediate  body,  the  challenge  being  to  control  both  the  motion  of  the  common 
wheel  axis  and  that  of  the  intermediate  body.  Interest  on  the  subject  was  probably 
promoted  by  the  US  patent  behind  the  Ginger  and  the  Segway  Human  Transporter 
projects.  Another  mobile  inverted  pendulum  is  known  as  JOE.  More  recently,  a 
new  class  of  nonholonomic  mechanical  systems  was  found  that  lies  somewhat 
between  holonomic  and  nonholonomic  systems;  these  systems  were  thus  termed 
quasiholonomic  (Ostrovskaya  and  Angeles  1998).  A  realization  of  this  class  was 
reported  by  Salerno  and  Angeles  (2004),  featuring  Quasimoro,  shown  in  Fig.  1.14, 
a  quasiholonomic  mobile  robot  intended  as  a  service  robot  for  the  motion-impaired. 
Quasimoro’s  central  body  is  to  carry  food,  drinks  and  books  to  the  user.  This  robot 
also  falls  within  the  category  of  MWP.  A  feature  common  to  this  category,  that  is 
not  encountered  in  other  wheeled  robots,  is  that  their  central  body,  which  plays  the 
role  of  the  robot  platform,  can  rotate  about  the  wheel  axis.  This  motion  should  be 
controlled,  thereby  leading  to  a  new  challenging  problem,  which  is  the  stabilization 
of  the  central  body,  aside  the  classical  control  problem  due  to  nonholonomy. 


1.5.2.1  Rovers 

Yet  another  class  of  wheeled  mobile  robots  is  known  as  rovers.  These  differ  from 
other  wheeled  robots  in  that  they  are  intended  for  uneven,  unstructured  terrain,  like 
that  found  off-road  on  the  Earth,  on  the  Moon  and  on  Mars.  The  latest  high-profile 
rover  is  NASA’s  Curiosity,  launched  from  Cape  Canaveral  on  November  26,  2011, 
having  landed  on  Mars  on  August  6,  2012.  As  rovers  are  intended  for  extraterrestrial 
exploration,  their  wheels  are  not  expected  to  roll  without  slipping;  instead,  they  are 
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Fig.  1.15  Rover  Chassis 
Prototype  (RCP)  undergoing 
tests  at  the  University  of 
Toronto  Institute  for 
Aerospace  Studies 
(UTIAS)Mars  Dome 
(courtesy  of  MDA 
Corporation,  Brampton,  ON) 


Fig.  1.16  A  computer  model 
of  the  Sojourner,  produced 
with  Vortex  (image  courtesy 
of  CM  Labs  Simulations  Inc.) 


designed  to  provide  enough  traction  in  the  presence  of  soft,  dry  terrain.  To  this 
end,  the  rover  wheels  are  supplied  with  grousers,  i.e.,  crests  protruding  from  the 
periphery  of  metal  wheels.  A  prototype  produced  by  MDA  for  planetary  exploration 
is  displayed  in  Fig.  1.15,  showing  its  six  wheels,  all  supplied  with  grousers.  A 
challenge  in  the  development  of  planetary  rovers  lies  in  that  tests  on  the  environment 
on  which  rovers  are  intended  to  roam  are  not  possible.  Conditions  are  emulated  on 
the  Earth  that  try  to  mimic  the  Moon’s  or  Mars’s  regolith.  One  instance  is  the  UTIAS 
Mars  Dome — see  Fig.  1.15.  Moreover,  the  mechanics  of  wheel-soil  interaction  can 
only  be  simulated  with  suitable  software.  CM  Labs’  Vortex  was  used  to  produce  the 
model  of  the  Sojourner  depicted  in  Fig.  1.16.  Using  this  model,  a  novel  approach 
was  introduced  to  represent  the  wheel-soil  interaction,  that  is  based  on  plasticity 
theory  (Azimi  et  al.  2012). 


1 .6  Swimming  Robots 
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Fig.  1.17  Aqua,  an 
amphibious  robot  (courtesy 
of  G.  Dudek/M.  Jenkin  on 
behalf  of  the  Aqua  Project) 


Fig.  1.18  Aqua,  swimming 
under  monitoring  by  its 
designer  (courtesy  of  G. 
Dudek/M.  Jenkin  on  behalf  of 
the  Aqua  Project) 


1.6  Swimming  Robots 

A  novel  class  of  robots  with  swimming  capabilities  is  currently  under  development 
in  various  research  laboratories.  Some  of  these  robots  have  been  designed  with 
the  morphology  of  fish  (Wen  et  al.  2011).  One  swimming  robot  designed  with  a 
hexapod  morphology,  featuring  six  flippers  in  lieu  of  legs  is  Aqua,  developed  at 
McGill  University,  and  depicted  in  Figs.  1.17  and  1.18.  The  latter  shows  Aqua  with 
its  designer,  Chris  Prahacs. 
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1.7  Flying  Robots 

This  field  is  becoming  quite  active,  with  some  robots  mimicking  the  morphology 
of  insects,  and  falling  into  the  category  of  micro-robots  Hines  et  al.  (2011);  other 
flying  robots  are  designed  as  unmanned  aerial  vehicles  (UAV)  Thorne  and  Yim 
(2011).  Intensive  research  is  now  being  reported  in  the  design  and  control  of 
two  novel  types  of  UAVs,  namely,  drones  and  quadrotors.  The  control  of  drones 
undergoing  fast  manuevers  has  been  made  possible  by  the  inception  of  gyro- 
free  inertial  measurement  systems  fabricated  with  MEMS^  technology.  Highly 
maneuverable  quadrotors,  especially  suitable  for  surveillance  and  reconnaissance 
missions,  are  small-size,  light-weight  flying  machines  supplied  with  “two  pairs  of 
counter-rotating  rotors  and  propellers,  located  at  the  vertices  of  a  square  frame” 
(Lee  et  al.  2010).  A  recent  issue  of  IEEE’s  Robotics  &  Automation  Magazine — 
Vol.  19,  No.  3,  September  2012 — includes  a  state-of-the-art  account  of  quadrotor 
technology. 


1.8  Exercises 

The  exercises  included  below  are  intended  to  familiarize  the  uninitiated  reader 
with  the  issues  involved  in  robotics,  especially  in  the  area  of  robotic  mechanical 
systems.  A  major  issue,  regrettably  quite  often  overlooked,  is  terminology.  In 
attempting  to  work  out  these  exercises,  the  beginner  should  be  able  to  better 
understand  the  language  of  robotics  and  realize  that  a  common  terminology  is 
not  yet  available.  Some  exercises  are  provided  as  an  aid  to  either  recall  or  learn 
fundamental  computational  issues  that  are  extremely  useful  in  the  development  of 
algorithms  for  the  analysis,  simulation  and  control  of  robotic  mechanical  systems. 

1 . 1  List  some  definitions  of  machine,  say  about  half  a  dozen,  trying  to  cover  the 
broadest  timespan  to  date.  Hint:  Hartenberg  and  Denavit  (1964)  list  a  few 
bibliographical  references. 

1.2  Try  to  give  an  answer  to  the  question:  Are  intelligent  machines  possible? 
Express  your  own  ideas  and  explore  what  scientists  like  Penrose  (1994)  think 
about  this  controversial  issue. 

1.3  What  is  the  difference  among  machine,  mechanism,  and  linkage!  In  particular, 
analyze  critically  the  definitions  given  by  authorities,  such  as  those  found 
in  the  most  respected  dictionaries,  encyclopedias,  and  archival  documents  of 
learned  societies,  e.g.,  the  complete  issue  of  Vol.  38,  Nos.  7-10  (2003)  of 
Mechanism  and  Machine  Theory  on  Standardization  of  Terminology. 

1.4  What  is  artificial  intelligence?  What  is  fuzzy  logic?  Can  the  techniques  of 
these  fields  be  applied  to  robotics? 


^ Microelectromechanical  systems. 
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1.5  What  is  mechatronics?  What  is  the  difference  between  mechatronics  and 
robotics?  Comerford  (1994)  and  Soureshi  et  al.  (1994)  give  an  account  on 
this  technology. 

1.6  What  do  you  understand  as  dexterity!  The  concept  of  dexterity  is  normally 
applied  to  persons.  Can  it  be  applied  to  animals  as  well?  What  about 
machines? 

1.7  Define  the  term  algorithm.  In  this  context,  make  a  clear  distinction  between 
recursion  and  iteration.  Note  that,  in  the  robotics  literature,  there  is  often 
confusion  between  these  two  terms  in  particular.  Make  sure  that  you  do  not 
make  the  same  mistake!  Again,  Penrose  (1994)  has  provided  an  extensive 
discussion  on  the  nature  of  algorithms. 

1.8  What  is  the  difference  among  terms  like  real-time,  on-line,  and  run-time! 

1.9  How  fast  can  two  floating-point  numbers  be  multiplied  using  a  personal 
computer?  What  about  using  a  UNIX  workstation?  a  supercomputer?  Write 
a  piece  of  code  to  estimate  this  time  on  your  computer  facility. 

1.10  Answer  the  foregoing  question  as  pertaining  to  floating-point  addition. 

1.11  What  is  the  smallest  floating-point  number  on  your  computer?  Rather  than 
looking  for  the  answer  in  manuals,  write  a  procedure  to  estimate  it. 

1.12  What  is  the  difference  between  procedural  programming  and  object-oriented 
programming!  In  terms  of  programming  languages,  what  is  the  difference 
between  C  and  C++?  Rumbaugh  et  al.  (1991)  provide  an  introduction  to 
object-oriented  programming,  while  Stroustrup  (1991)  gives  an  introduction 
to  CH — h- 


Chapter  2 

Mathematical  Background 


2.1  Preamble 

First  and  foremost,  the  study  of  motions  undergone  by  robotic  mechanical  systems 
or,  for  that  matter,  by  mechanical  systems  at  large,  requires  a  suitable  motion 
representation.  Now,  the  motion  of  mechanical  systems  involves  the  motion  of 
the  particular  links  comprising  those  systems,  which  in  this  book  are  supposed 
to  be  rigid.  The  assumption  of  rigidity,  although  limited  in  scope,  still  covers  a 
wide  spectrum  of  applications,  while  providing  insight  into  the  motion  of  more 
complicated  systems,  such  as  those  involving  deformable  bodies. 

The  most  general  kind  of  rigid-body  motion  consists  of  both  translation  and 
rotation.  While  the  study  of  the  former  is  covered  in  elementary  mechanics  courses 
and  is  reduced  to  the  mechanics  of  particles,  the  latter  is  more  challenging.  Indeed, 
point  translation  can  be  studied  simply  with  the  aid  of  three-dimensional  vector 
calculus,  while  rigid-body  rotations  require  the  introduction  of  tensors,  i.e.,  entities 
mapping  vector  spaces  into  vector  spaces. 

Emphasis  is  placed  on  invariant  concepts,  i.e.,  items  that  do  not  change  upon  a 
change  of  coordinate  frame.  Examples  of  invariant  concepts  are  geometric  quantities 
such  as  distances  and  angles  between  lines.  Although  we  may  resort  to  a  coordinate 
frame  and  vector  algebra  to  compute  distances  and  angles,  and  will  represent  vectors 
in  that  frame,  the  final  result  will  be  independent  of  how  we  choose  that  frame.  The 
same  applies  to  quantities  whose  evaluation  calls  for  the  introduction  of  tensors. 
Here,  we  must  distinguish  between  the  physical  quantity  represented  by  a  vector  or 
a  tensor  and  the  representation  of  that  quantity  in  a  coordinate  frame  using  a  one¬ 
dimensional  array  of  components  in  the  case  of  vectors,  or  a  two-dimensional  array 
in  the  case  of  tensors.  It  is  unfortunate  that  the  same  word  is  used  in  English  to 
denote  a  vector  and  its  array  representation  in  a  given  coordinate  frame.  Regarding 
tensors,  the  associated  arrays  are  called  matrices.  By  abuse  of  terminology,  we  will 
refer  to  both  tensors  and  their  arrays  as  matrices,  although  keeping  in  mind  the 
essential  conceptual  differences  involved. 
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2.2  Linear  Transformations 

The  physical  three-dimensional  space  is  a  particular  case  of  a  vector  space. 
A  vector  space  is  a  set  of  objects,  called  vectors,  that  follow  certain  algebraic  rules. 
Throughout  the  book,  vectors  will  be  denoted  by  boldface  lowercase  characters, 
whereas  tensors  and  their  matrix  representations  will  be  denoted  by  boldface 
uppercase  characters.  Let  v,  vi,  V2,  V3,  and  w  be  elements  of  a  given  vector  space  V, 
which  is  defined  over  the  real  field,  and  let  a  and  P  be  two  elements  of  this  field,  i.e., 
a  and  P  are  two  real  numbers.  Below  we  summarize  the  rules  mentioned  above: 

(i)  The  sum  of  vi  and  V2,  denoted  by  vi  +  V2,  is  itself  an  element  of  V  and  is 
commutative,  i.e.,  Vi  +  V2  =  V2  +  Vi; 

(ii)  V  contains  an  element  0,  called  the  zero  vector  of  V,  which,  when  added  to 
any  other  element  v  of  V,  leaves  it  unchanged,  i.e.,  v  +  0  =  v; 

(iii)  The  sum  defined  in  (i)  is  associative,  i.e.,  Vi  +  (v2  +  V3)  =  (vi  +  V2)  +  V3; 

(iv)  For  every  element  v  of  V,  there  exists  a  corresponding  element,  w,  also  of  V, 
which,  when  added  to  v,  produces  the  zero  vector,  i.e.,  v  +  w  =  0.  Moreover, 
w  is  represented  as  —  v; 

(v)  The  product  ay,  or  ya,  is  also  an  element  of  V,  for  every  v  of  V  and  every  real 
a.  This  product  is  associative,  i.e.,  a(Py)  =  (aP)y; 

(vi)  If  a  is  the  real  unity,  then  av  is  identically  v; 

(vii)  The  product  defined  in  (v)  is  distributive  in  the  sense  that  {a)  {a  +  P)y  = 
ay  +  Py  and  {b)  ^(vi  +  V2)  =  ayi  +  ay2. 

Although  vector  spaces  can  be  defined  over  other  fields,  we  will  deal  with 
vector  spaces  over  the  real  field,  unless  explicit  reference  to  another  field  is  made. 
Moreover,  vector  spaces  can  be  either  finite-  or  infinite-dimensional,  but  we  will  not 
need  the  latter.  In  geometry  and  elementary  mechanics,  the  dimension  of  the  vector 
spaces  needed  is  usually  three,  but  when  studying  multibody  systems,  an  arbitrary 
finite  dimension  will  be  required.  The  concept  of  dimension  of  a  vector  space  is 
discussed  in  more  detail  later. 

A  linear  transformation,  represented  as  an  operator  L,  of  a  vector  space  U  into 
a  vector  space  V,  is  a  rule  that  assigns  to  every  vector  u  of  Z//  at  least  one  vector  v  of 
V,  represented  as  v  =  Lu,  with  L  endowed  with  two  properties: 

(i)  homogeneity:  L(q'u)  =  ay',  and 

(ii)  additivity:  L(ui  +  U2)  =  Vi  +  V2. 

Note  that,  in  the  foregoing  definitions,  no  mention  has  been  made  of  components, 
and  hence,  vectors  and  their  transformations  should  not  be  confused  with  their  array 
representations. 

Particular  types  of  linear  transformations  of  the  three-dimensional  Euclidean 
space  that  will  be  encountered  frequently  in  this  context  are  projections,  reflections, 
and  rotations.  One  further  type  of  transformation,  which  is  not  linear,  but  neverthe¬ 
less  appears  frequently  in  kinematics,  is  the  one  known  as  affine  transformation.  The 
foregoing  transformations  are  defined  below.  It  is  necessary,  however,  to  introduce 
additional  concepts  pertaining  to  general  linear  transformations  before  expanding 
into  these  definitions. 


2.2  Linear  Transformations 
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Fig.  2.1  A  projection  onto  a 
plane  77  of  unit  normal  n 


The  range  of  a  linear  transformation  of  hi  into  V  is  the  set  of  vectors  v  of  V 
into  which  some  vector  u  of  Z//  is  mapped,  i.e.,  the  range  of  L  is  defined  as  the  set 
of  V  =  Lu,  for  every  vector  n  of  U.  The  kernel  of  L  is  the  set  of  vectors  u^y  of 
U  that  are  mapped  by  L  into  the  zero  vector  0  G  V.  It  can  be  readily  proven  (see 
Exercises  2. 1-2.3)  that  the  kernel  and  the  range  of  a  linear  transformation  are  both 
vector  subspaces  of  lA  and  V,  respectively,  i.e.,  they  are  themselves  vector  spaces, 
but  of  a  dimension  smaller  than  or  equal  to  that  of  their  associated  vector  spaces. 
Moreover,  the  kernel  of  a  linear  transformation  is  often  called  the  null  space  of  the 
said  transformation. 

Henceforth,  the  three-dimensional  Euclidean  space  is  denoted  by  .  Having 
chosen  an  origin  O  for  this  space,  its  geometry  can  be  studied  in  the  context 
of  general  vector  spaces.  Hence,  points  of  8^  will  be  identified  with  vectors  of 
the  associated  three-dimensional  vector  space.  Moreover,  lines  and  planes  passing 
through  the  origin  are  subspaces  of  dimensions  1  and  2,  respectively,  of  8^ .  Clearly, 
lines  and  planes  not  passing  through  the  origin  of  8^  are  not  subspaces  but  can  be 
handled  with  the  algebra  of  vector  spaces,  as  will  be  shown  here. 

An  orthogonal  projection  P  of  8^  onto  itself  is  a  linear  transformation  of  the  said 
space  onto  a  plane  77  passing  through  the  origin  and  having  a  unit  normal  n,  with 
the  properties: 


=  P,  Pn  =  0  (2.1a) 

Any  matrix  with  the  first  property  above  is  termed  idempotent.  Eor  n  y^n  matrices, 
it  is  sometimes  necessary  to  indicate  the  lowest  integer  /  for  which  an  analogous 
relation  follows,  i.e.,  for  which  P^  =  P.  In  this  case,  the  matrix  is  said  to  be 
idempotent  of  degree  / . 

Clearly,  the  projection  of  a  position  vector  p,  denoted  by  p',  onto  a  plane  77  of 
unit  normal  n,  is  p  itself  minus  the  component  of  p  along  n  as  shown  in  Eig.  2.1,  i.e., 

p'  =  p  —  n(n^p)  (2.1b) 

where  the  superscript  T  denotes  either  vector  or  matrix  transposition  and  n^p  is 
equivalent  to  the  usual  dot  product  n  •  p. 
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Fig.  2.2  A  reflection  onto  a 
plane  77  of  unit  normal  n 


Now,  the  identity  matrix  1  is  defined  as  the  mapping  of  a  vector  space  V  into 
itself  leaving  every  vector  v  of  V  unchanged,  i.e.. 


(2.2) 


Thus,  p',  as  given  by  Eq.  (2.1b),  can  be  rewritten  as 

p'  =  Ip  —  nn^p  =  (1  —  nn^)p  (2.3) 

and  hence,  the  orthogonal  projection  P  onto  77  can  be  represented  as 

P  =  1  -  nn^  (2.4) 

where  the  product  nn^  amounts  to  a  3  x  3  matrix. 

Now  we  turn  to  reflections.  Here  we  have  to  take  into  account  that  reflections 
occur  frequently  accompanied  by  rotations,  as  yet  to  be  studied.  Since  reflections 
are  simpler  to  represent,  we  first  discuss  these,  rotations  being  discussed  in  full 
detail  in  Sect.  2.3.  What  we  shall  discuss  in  this  section  is  pure  reflections,  i.e., 
those  occurring  without  any  concomitant  rotation.  Thus,  all  reflections  studied  in 
this  section  are  pure  reflections,  but  for  the  sake  of  brevity,  they  will  be  referred  to 
simply  as  reflections. 

A  reflection  R  of  onto  a  plane  FI  passing  through  the  origin  and  having  a 
unit  normal  n  is  a  linear  transformation  of  the  said  space  into  itself,  as  depicted  in 
Fig.  2.2,  such  that  a  vector  p  is  mapped  by  R  into  a  vector  p'  given  by 

p'  =  p  —  2nn^p  =  (1  —  2nn^)p 

Thus,  the  reflection  R  can  be  expressed  as 

R  =  1  -  2nn^ 


(2.5) 
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From  Eq.  (2.5)  it  is  then  apparent  that  a  pure  reflection  is  represented  by  a  linear 
transformation  that  is  symmetric  and  whose  square  equals  the  identity  matrix,  i.e., 
=  1.  Indeed,  symmetry  is  apparent  from  the  equation  above;  the  second  property 
is  readily  proven  below: 

r2  =  (1  -  2nn^)(l  -  2nn^) 

=  1  —  2nn^  —  2nn^  +  4(nn^)(nn^)  =  1  —  4nn^  +  4n(n^n)n^ 

which  apparently  reduces  to  1  because  n  is  a  unit  vector.  Note  that  from  the  second 
property  above,  we  And  that  pure  reflections  observe  a  further  interesting  property, 
namely, 

R“‘  =  R 

i.e.,  every  pure  reflection  equals  its  inverse.  This  result  can  be  understood  intuitively 
by  noticing  that,  upon  doubly  reflecting  an  image  using  two  mirrors,  the  original 
image  is  recovered.  Any  square  matrix  which  equals  its  inverse  will  be  termed  self¬ 
inverse  henceforth. 

Further,  we  take  to  deriving  the  orthogonal  decomposition  of  a  given  vector  v 
into  two  components,  one  along  and  one  normal  to  a  unit  vector  e.  The  component 
of  V  along  e,  termed  here  the  axial  component,  vy — read  v-par — is  simply  given  as 

V||  =  ee^v  (2.6a) 

while  the  corresponding  normal  component,  v_l — read  v-perp — is  simply  the 
difference  v  —  vy,  i.e., 

v_L  =  V  —  vy  =  (1  —  ee^)v  (2.6b) 

the  matrix  in  parentheses  in  the  foregoing  equation  being  rather  frequent  in 
kinematics.  This  matrix  will  appear  when  studying  rotations. 

Further  concepts  are  now  recalled:  The  basis  of  a  vector  space  V  is  a  set  of 
linearly  independent  vectors  of  V,  {v, in  terms  of  which  any  vector  v  of  V  can  be 
expressed  as 


V  =  ai\i  +  0'2V2  H - h  0in\n  (2.7) 

where  the  elements  of  the  set  {o'/}"  are  all  elements  of  the  held  over  which  V  is 
deflned,  i.e.,  they  are  real  numbers  in  the  case  at  hand.  The  number  n  of  elements 
in  the  set  B  =  {v/ is  called  the  dimension  of  V.  Note  that  any  set  of  n  linearly 
independent  vectors  of  V  can  play  the  role  of  a  basis  of  this  space,  but  once  this 
basis  is  deflned,  the  set  of  real  coefficients  {at representing  a  given  vector  v  is 
unique. 

Let  U  and  V  be  two  vector  spaces  of  dimensions  m  and  n,  respectively,  and  L  a 
linear  transformation  of  U  into  V,  and  deflne  bases  Bu  and  By  for  U  and  V  as 


Bu  =  {nj}f,  By  =  {yiri 


(2.8) 
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Since  each  Luy  is  an  element  of  V,  it  can  be  represented  uniquely  in  terms  of  the 
vectors  of  By,  namely,  as 


Luy  =  /ly Vi  +  hjyi  H - h  Inj^n,  j  =  1, . . . ,  m  (2.9) 

Consequently,  in  order  to  represent  the  images  of  the  m  vectors  of  Bu,  namely, 
the  set  {Luy  n  x  m  real  numbers  Uj ,  for  /  =  1, . . . ,  and  j  =  1, . . . ,  m,  are 
necessary.  These  real  numbers  are  now  arranged  in  the  n  x  m  array  defined 

below: 


^11  hi  •  •  •  hm 
hi  hi  •  •  •  hm 


(2.10) 


The  foregoing  array  is  thus  called  the  matrix  representation  of  L  with  respect  to 
Bu  and  By.  We  thus  have  an  important  definition: 

Definition  2.2.1.  The  y  th  column  of  the  matrix  representation  of  L  with  respect  to 
the  bases  Bjj  and  By  is  composed  of  the  n  real  coefficients  Uj  of  the  representation 
of  the  image  of  the  y  th  vector  of  Bu  in  terms  of  By. 

The  notation  introduced  in  Eq.  (2.10)  is  rather  cumbersome,  for  it  involves  one 
subscript  and  one  superscript.  Moreover,  each  of  these  is  subscripted.  In  practice, 
the  bases  involved  are  self-evident,  which  makes  an  explicit  mention  of  these 
unnecessary.  In  particular,  when  L  is  a  mapping  of  U  onto  itself,  a  single  basis 
suffices  to  represent  L  in  matrix  form.  In  this  case,  its  bracket  will  bear  only  a 
subscript,  and  no  superscript,  namely,  [L]23.  Moreover,  we  will  use,  henceforth,  the 
concept  of  basis  and  coordinate  frame  interchangeably,  since  one  implies  the  other. 

Two  different  bases  are  unavoidable  when  the  two  spaces  under  study  are 
physically  distinct,  which  is  the  case  in  velocity  analysis  of  manipulators.  As  we 
will  see  in  Chap.  5,  in  these  analyses  we  distinguish  between  the  velocity  of 
the  manipulator  in  Cartesian  space  and  that  in  the  joint-rate  space.  While  the 
Cartesian-space  velocity — or  Cartesian  velocity,  for  brevity — consists,  in  general, 
of  a  six-dimensional  vector  containing  the  three-dimensional  angular  velocity  of 
the  end-effector  and  the  translational  velocity  of  one  of  its  points,  the  latter  is  an 
-dimensional  vector.  Moreover,  if  the  manipulator  is  coupled  by  revolute  joints 
only,  the  units  of  the  joint-rate  vector  are  all  s“\  whereas  the  Cartesian  velocity 
contains  some  components  with  units  of  s“^  and  others  with  units  of  ms“^ 

Further  definitions  are  now  recalled.  Given  a  mapping  L  of  an  -dimensional 
vector  space  lA  into  the  w -dimensional  vector  space  V,  a  nonzero  vector  e  that  is 
mapped  by  L  into  a  multiple  of  itself,  Ae,  is  called  an  eigenvector  of  L,  the  scalar 
X  being  called  an  eigenvalue  of  L.  The  eigenvalues  of  L  are  determined  by  the 
equation 


det(Al  -  L)  =  0 


(2.11) 
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Note  that  the  matrix  A1 — L  is  linear  in  A,  and  since  the  determinant  of  a  x  matrix 
is  a  homogeneous  /ith-order  function  of  its  entries,  the  left-hand  side  of  Eq.  (2.1 1)  is 
a  /2th-degree  polynomial  in  A.  The  foregoing  polynomial  is  termed  the  characteristic 
polynomial  of  L.  Hence,  every  n  xn  matrix  L  has  n  complex  eigenvalues,  even  if  L  is 
defined  over  the  real  field.  If  it  is,  then  its  complex  eigenvalues  appear  in  conjugate 
pairs.  Clearly,  the  eigenvalues  of  L  are  the  roots  of  its  characteristic  polynomial, 
while  Eq.  (2.1 1)  is  called  the  characteristic  equation  of  L. 

Example  2.2.1.  What  is  the  representation  of  the  reflection  R  of  into  itself,  with 
respect  to  the  x-y  plane,  in  terms  of  unit  vectors  parallel  to  the  X,  Y,  Z  axes  that 
form  a  coordinate  frame  IFl 

Solution:  Note  that  in  this  case,  U  =  V  =  £  and,  hence,  it  is  not  necessary  to  use 
two  different  bases  for  U  and  V.  Now,  let  i,  j,  k,  be  unit  vectors  parallel  to  the  X,  T, 
and  Z  axes.  Clearly, 


Ri  =  i 

Rj=j 

Rk  =  -k 

Thus,  the  representations  of  the  images  of  i,  j  and  k  under  R,  in  IF,  are 


1 

"O' 

'  0  ■ 

[Ri]^  = 

1 

o  o 
_ 1 

,  [Rj]^  = 

1 

_0_ 

,  [Rkb  = 

0 

_-l_ 

where  subscripted  brackets  are  used  to  indicate  the  representation  frame.  Hence,  the 
matrix  representation  of  R  in  F,  denoted  by  [  R  ] jr,  is 


1  0  0 

0  1  0 

0  0-1 
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A  linear  isomorphism,  i.e.,  a  one-to-one  linear  transformation  mapping  a  space  V 
onto  itself,  is  called  an  isometry  if  it  preserves  distances  between  any  two  points 
of  V.  If  u  and  v  are  regarded  as  the  position  vectors  of  two  such  points,  then  the 
distance  d  between  these  two  points  is  defined  as 


d  =  y(u  -  v)^(u  v) 


(2.12) 
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The  volume  V  of  the  tetrahedron  defined  by  the  origin  and  three  points  of  the 
three-dimensional  Euclidean  space  of  position  vectors  u,  v,  and  w  is  obtained  as 
one-sixth  of  the  absolute  value  of  the  double  mixed  product  of  these  three  vectors, 


1 

E  =  -  u  X  V  •  w 
6 


det  [u  V  w] 


(2.13) 


i.e.,  if  a  3x3  array  [A]  is  defined  in  terms  of  the  components  of  u,  v,  and  w,  in  a 
given  basis,  then  the  first  column  of  [A]  is  given  by  the  three  components  of  u,  the 
second  and  third  columns  being  defined  likewise. 

Now,  let  Q  be  an  isometry  mapping  the  triad  {u,  v,  w}  into  {u',  v',  w'}. 
Moreover,  the  distance  from  the  origin  to  the  points  of  position  vectors  u,  v,  and 
w  is  given  simply  as  ||u||,  ||v||,  and  ||w||,  which  are  defined  as 


Clearly, 


and 


(2.14) 


(2.15a) 


det  [u'  v'  w']  =  ibdet  [u  v  w] 


(2.15b) 


If,  in  the  foregoing  relations,  the  sign  of  the  determinant  is  preserved,  the 
isometry  represents  a  rotation',  otherwise,  it  represents  a  reflection.  Now,  let  p  be 
the  position  vector  of  any  point  of  ,  its  image  under  a  rotation  Q  being  p'.  Hence, 
distance  preservation  requires  that 


P^P  =  p'^p'  (2.16) 

where 

p'  =  Qp  (2.17) 

condition  (2.16)  thus  leading  to 

Q^Q  =  1  (2.18) 

where  1  was  defined  in  Sect.  2.2  as  the  3x3  identity  matrix,  and  hence,  Eq.  (2.18) 
states  that  Q  is  an  orthogonal  matrix.  Moreover,  let  T  and  T'  denote  the  two  matrices 
defined  below: 


T  =  [u  V  w] ,  T'  =  [u'  v'  w'] 


(2.19) 


2.3  Rigid-Body  Rotations 


35 


from  which  it  is  clear  that 


T'  =  QT 


(2.20) 


Now,  for  a  rigid-body  rotation,  Eq.  (2.15b)  should  hold  with  the  positive  sign, 
and  hence. 


det(T)  =  det(T0  (2.21a) 

and,  by  virtue  of  Eq.  (2.20),  we  conclude  that 

det(Q)  =  +1  (2.21b) 

Therefore,  Q  is  a  proper  orthogonal  matrix,  i.e.,  it  is  a  proper  isometry.  Now  we 
have 

Theorem  2.3.1.  The  eigenvalues  of  a  proper  orthogonal  matrix  Q  lie  on  the  unit 
circle  centered  at  the  origin  of  the  complex  plane. 

Proof  Let  X  be  one  of  the  eigenvalues  of  Q  and  e  the  corresponding  eigenvector, 
so  that 


Qe  =  Ae  (2.22) 

In  general,  Q  is  not  expected  to  be  symmetric,  and  hence,  X  is  not  necessarily  real. 
Thus,  X  is  considered  complex,  in  general.  In  this  light,  when  transposing  both  sides 
of  the  foregoing  equation,  we  will  need  to  take  the  complex  conjugates  as  well. 
Henceforth,  the  complex  conjugate  of  a  vector  or  a  matrix  will  be  indicated  with 
an  asterisk  as  a  superscript.  As  well,  the  conjugate  of  a  complex  variable  will  be 
indicated  with  a  bar  over  the  said  variable.  Thus,  the  transpose  conjugate  of  the 
above  equation  takes  on  the  form 


(2.23) 


Multiplying  the  corresponding  sides  of  the  two  previous  equations  yields 


e*Q*Qe  =  AAe*e  (2.24) 

However,  Q  has  been  assumed  real,  and  hence,  Q*  reduces  to  Q^,  the  foregoing 
equation  thus  reducing  to 

e*Q^Qe  =  XAe*e  (2.25) 

But  Q  is  orthogonal  by  assumption,  and  hence,  it  obeys  Eq.  (2.18),  which  means 
that  Eq.  (2.25)  reduces  to 


e 


* 


e  = 


(2.26) 
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where  |  •  |  denotes  the  module  of  the  complex  variable  within  it.  Thus,  the  foregoing 
equation  leads  to 


(2.27) 


thereby  completing  the  intended  proof.  As  a  direct  consequence  of  Theorem  2.3.1, 
we  have 

Corollary  2.3.1.  A  proper  orthogonal  3x3  matrix  has  at  least  one  eigenvalue  that 
is  +1. 

Now,  let  e  be  the  eigenvector  of  Q  associated  with  the  eigenvalue  +1.  Thus, 

Qe  =  e  (2.28) 


What  Eq.  (2.28)  states  is  summarized  as  a  theorem  below: 

Theorem  2.3.2  (Euler  1776).  A  rigid-body  motion  about  a  point  O  leaves  fixed 
a  set  of  points  lying  on  a  line  C  that  passes  through  O  and  is  parallel  to  the 
eigenvector  e  of  Q  associated  with  the  eigenvalue  +1. 

A  further  result,  that  finds  many  applications  in  robotics  and,  in  general,  in 
system  theory,  is  given  below: 

Theorem  2.3.3  (Cayley-Hamilton).  Let  P{X)  be  the  characteristic  polynomial  of 
an  xn  matrix  A,  i.e., 

P{X)  =  det(Al  -  A)  =  A"  +  a„_iA''“‘  +  •  •  •  +  aiA  +  aq  (2.29) 

Then  A  satisfies  its  characteristic  equation,  i.e., 

A" +a„_iA"“‘ +---  +  aiA  +  aol  =  O  (2.30) 

where  O  is  the  n  xn  zero  matrix. 

Proof.  See  Kaye  and  Wilson  (1998). 

What  the  Cayley-Hamilton  Theorem  states  is  that  any  power  p  >  n  oi  the 
n  X  n  matrix  A  can  be  expressed  as  a  linear  combination  of  the  first  n  powers  of 
A — the  0th  power  of  A  is,  of  course,  the  n  x  n  identity  matrix  1.  An  important 
consequence  of  this  result  is  that  any  analytic  matrix  function  of  A  can  be  expressed 
not  as  an  infinite  series,  but  as  a  sum,  namely,  a  linear  combination  of  the  first  n 
powers  of  A:  1,  A,  . . . ,  A"“^.  An  analytic  function  / (x)  of  a  real  variable  x  is,  in 
turn,  a  function  with  a  series  expansion.  Moreover,  an  analytic  matrix  function  of 
a  matrix  argument  A  is  defined  likewise,  an  example  of  which  is  the  exponential 
function.  From  the  previous  discussion,  then,  the  exponential  of  A  can  be  written  as 
a  linear  combination  of  the  first  n  powers  of  A.  It  will  be  shown  later  that  any  proper 
orthogonal  matrix  Q  can  be  represented  as  the  exponential  of  a  skew- symmetric 
matrix  derived  from  the  unit  vector  e  of  Q,  of  eigenvalue  +1,  and  the  associated 
angle  of  rotation,  as  yet  to  be  defined. 
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2.3.1  The  Cross-Product  Matrix 

Prior  to  introducing  the  matrix  representation  of  a  rotation,  we  will  need  a  few 
definitions.  We  will  start  by  defining  the  partial  derivative  of  a  vector  with  respect 
to  another  vector.  This  is  a  matrix,  as  described  below:  In  general,  let  u  and  v  be 
vectors  of  spaces  lA  and  V,  of  dimensions  m  and  n,  respectively.  Furthermore,  let 
^  be  a  real  variable  and  /  be  real- valued  function  of  u  =  u(0  and  v  = 
being  m-  and  ^-dimensional  vector  functions  of  t  as  well,  with  /  =  /(u,  v).  The 
derivative  of  u  with  respect  to  t,  denoted  by  u(0^  is  a  m -dimensional  vector  whose 
ith  component  is  the  derivative  of  the  ith  component  ut  of  u,  in  a  given  basis,  with 
respect  to  A  similar  definition  follows  for  \(t).  The  partial  derivative  of  /  with 
respect  to  u  is  a  m -dimensional  vector  whose  ith  component  is  the  partial  derivative 
of  /  with  respect  to  ut ,  with  a  corresponding  definition  for  the  partial  derivative  of 
/  with  respect  to  v.  The  foregoing  derivatives,  as  all  other  vectors,  will  be  assumed, 
henceforth,  to  be  column  arrays.  Thus, 


df/dui 

df/dvi 

9/  _ 

a//  du2 

9/  _ 

dfldv2 

9u 

_df/dum_ 

9v 

_df/dv„_ 

(2.31) 


Furthermore,  let  v  =  v(u).  In  order  to  derive  9u/  9v,  first  the  differential  dv  upon 
a  differential  du  is  computed: 


dv  = 


or,  in  array  form. 


dv  = 


9v  9v  9v 

dui  du2  dujfi 

“vr 

9v 

9u 


dui 

du2 


(2.32a) 


(2.32b) 


That  is,  the  partial  derivative  of  v  with  respect  to  u  is  a  x  m  array  whose  (/,  j) 
entry  is  defined  as  dvi  /  duj ,  i.e.. 


9v 


dvi/dui  dvildu2  •••  dvijdum 
dv2ldui  dv2ldu2  •••  dv2ldum 


dVnldui  dVnldu2  •••  dVnIdUm 


9u 


(2.33) 
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Hence,  the  total  derivative  of  /  with  respect  to  u  can  be  written  as 

dn  9u  V  9u  /  9v 


(2.34) 


If,  moreover,  /  is  an  explicit  function  of  t,  i.e.,  if  /  =  /(u,  v,  t)  and  v  = 
v(u,  t),  then,  one  can  write  the  total  derivative  of  /  with  respect  to  t  as 


df_ 

dt 


^  9v  dn 
9u  dt 


(2.35) 


The  total  derivative  of  v  with  respect  to  t  can  be  written,  likewise,  as 


d\  9v  d\  dn 
dt  dt  9u  dt 


(2.36) 


Example  2.3.1.  Let  the  components  of  v  and  x  in  a  certain  reference  frame  T  be 
given  as 


= 

~Vl~ 

V2 

-  [x]^  = 

Xi 

X2 

_^3_ 

_-^3_ 

Then 


Hence, 


[vxx]^  = 


VlX^  -  V3X2 
V3XI  -  VIX3 
VIX2  -  V2XI 


’  9(v  X  x)  ' 

9x 

T 

0 

V3 


-V3 

0 


V2 

-Vi 

0 


(2.37a) 


(2.37b) 


(2.37c) 


Henceforth,  the  partial  derivative  of  v  x  x  with  respect  to  x  will  be  denoted  by  the 
3x3  matrix  V.  For  obvious  reasons,  V  is  termed  the  cross-product  matrix  of  vector 
V.  Sometimes  the  cross-product  matrix  of  a  vector  v  is  represented  as  v,  but  we  do 
not  follow  this  notation  for  the  sake  of  consistency,  since  we  decided  at  the  outset 
to  represent  matrices  with  boldface  uppercases.  Thus,  the  foregoing  cross  product 
admits  the  alternative  representations 

V  X  X  =  Vx  (2.38) 


Now,  it  should  be  apparent  that: 
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Theorem  2.3.4.  The  cross-product  matrix  A  of  any  three-dimensional  vector  a  is 
skew -symmetric,  i.e., 


A 


T 


and,  as  a  consequence. 


a  X  (a  X  b)  =  A^b 


where  A?  can  be  readily  proven  to  be 


a|  +  aa 


T 


(2.39) 


(2.40) 


with 


denoting  the  Euclidean  norm  of  the  vector  inside  it. 


Note  that  given  any  three-dimensional  vector  a,  its  cross-product  matrix  A  is 
uniquely  defined.  Moreover,  this  matrix  is  skew- symmetric.  The  converse  also 
holds,  i.e.,  given  any  3x3  skew- symmetric  matrix  A,  its  associated  vector  is 
uniquely  defined  as  well.  This  result  is  made  apparent  from  Example  2.3.1  and  will 
be  discussed  further  when  we  define  the  axial  vector  of  an  arbitrary  3x3  matrix 
below. 


Example  2.3.2.  Let  a  be  an  arbitrary  three-dimensional  vector  and  A  its  cross- 
product  matrix.  Eurther,  let  B  =  1  +  A,  with  1  defined  as  the  3x3  identity  matrix. 

Without  resorting  to  components, 

(a)  prove  that 


det(B)  =  1  + 


>  1 


and  hence,  B  is  nonsingular. 

(b)  EindB-^  in  terms  of  A  or,  equivalently,  of  a. 

Solution: 

(a)  Let  { at  be  the  set  of  eigenvalues  and  { a/  the  set  of  corresponding 
eigenvectors  of  A.  Likewise,  let  {  ^  be  the  set  of  eigenvalues  and  {  b/  } ^  the 

set  of  corresponding  eigenvectors  of  B.  That  is, 

Aa,-  =  O', a,-,  Bb/  =  ^,b,-,  i  =  1,2,3 
Next,  add  a^  to  both  sides  of  the  first  of  the  above  equations: 

at  +  Aa,-  =  a,-  +  a, -a,-  ^  (1  +  A)a,-  =  (1  +  a^a,- ,  i  =  1,2,3 


But,  in  light  of  the  definition  of  B,  the  second  of  the  above  equations  leads  to 

Ba,  =  (1  +  ai)ai,  i  =  1,2,3 
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which  means  that  b/  =  a,  and  =  l+o'/,/  =  1, 2,  3.  Now,  det(B)  =  P1P2P3, 
and  hence, 


det(B)  =  (1  +  aOCl  +  q'2)(1  +  «3) 

Therefore,  to  find  the  eigenvalues  of  A  =  CPM(a),  notice  that  A  can  be 
written  as 


A  =  CPM(a)  =  CPM(||a||e),  e  = 


a 


a 


i.e.,  e  is  a  unit  vector  obtained  upon  dividing  a  by  its  norm.  By  virtue  of  the 
definition  of  the  cross-product  matrix,  moreover,  the  factor  ||a||  can  be  taken 
outside  of  the  CPM(  • )  operator,  and  hence. 


A  =  ||a||CPM(e)  =  ||a||E,  E  =  CPM(e) 


But  the  eigenvalues  of  the  CPM  of  a  unit  vector  are  proven  in  Exercise  2.11  to 
be  0,  j  and  — 7 ,  with  j  =  \/^.  Therefore,  ai  =  0,  0^2  =  7  ||a| 
and  hence,  =  1,  ^2  =  1  +  7  ||a||,  P3  =  I  —  j  ||a||.  Thus, 


«3  =  -J 


a 


det(B)  =  1(1  +  i||a||)(l  -y||a||)  =  1  +  ||a 


thereby  completing  the  intended  proof. 

(b)  Now,  to  find  B“\  the  characteristic  equations  of  A  and  B  will  be  needed.  These 
are  readily  derived  below: 


A:  (A  —  ai){X  —  0:2) (A  —  a^)  =  0 


A^  +  I  alPA  =  0 


and 


B:  (X-Pi)(X-P2)(^-P3)  =  0  A3-3A2  +  (3  +  ||af)A-(l  +  ||af)  =  0 


If  now  the  Cayley-Hamilton  Theorem  is  invoked,  the  foregoing  scalar 
characteristic  equations  lead  to  matrix  polynomials  in  A  and  B,  namely. 


+  ||af  A  =  O,  B^  -  3B2  +  (3  +  ||af  )B  -  (1  +  ||af  )1  =  O 


with  O  denoting  the  3x3  zero  matrix.  Since  B  is  known  to  be  non- singular 
from  (a),  the  two  sides  of  the  polynomial  in  B  can  be  multiplied  by  B“^;  the 
last  term  of  the  right-hand  side  of  the  matrix  polynomial  thus  resulting  involves 
B“^  Upon  solving  for  this  term. 
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Fig.  2.3  Rotation  of  a  rigid  body  about  a  line 


which,  upon  expansion,  yields 


B 


-1 


1  +  lla| 


[(1  +  I|a|l4l 


A  +  A^] 


The  reader  should  be  able  to  verify  that  the  foregoing  expression  is  indeed  the 
inverse  of  1  +  A. 


2.3.2  The  Rotation  Matrix 

In  deriving  the  matrix  representation  of  a  rotation,  we  should  recall  Theorem  2.3.2, 
which  suggests  that  an  explicit  representation  of  Q  in  terms  of  its  eigenvector  e 
is  possible.  Moreover,  this  representation  must  contain  information  on  the  amount 
of  the  rotation  under  study,  which  is  nothing  but  the  angle  of  rotation.  Furthermore, 
line  £,  mentioned  in  Euler’s  Theorem,  is  termed  the  axis  of  rotation  of  the  motion  of 
interest.  In  order  to  derive  the  representation  mentioned  above,  consider  the  rotation 
depicted  in  Fig.  2.3  of  angle  f  about  line  C. 

From  Fig.  2.3a,  one  can  apparently  write 

p'  ^  (2.41) 


where  OQ  is  the  axial  component  of  p  along  vector  e,  which  is  derived  as  in 
Eq.  (2.6a),  namely, 

Tq=  ee^p  (2.42) 

Furthermore,  from  Fig.  2.3b, 

- ^  ^  ^ 

QP'=  (cos0)  QP  +(sin0)  QP" 


(2.43) 
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with  QP  being  nothing  but  the  normal  component  of  p  with  respect  to  e,  as 
introduced  in  Eq.  (2.6b),  i.e., 


QP={\-  ee^)p  (2.44) 


and  QP"  given  as 


er’"=exp  =  Ep  (2.45) 

Substitution  of  Eqs.  (2.44)  and  (2.45)  into  Eq.  (2.43)  leads  to 


QP '  =  cos  0  (1  —  ee^)p  +  sin  0Ep  (2.46) 

If  now  Eqs.  (2.42)  and  (2.46)  are  substituted  into  Eq.  (2.41),  one  obtains 

p'  =  ee^p  +  cos  0(1  —  ee^)p  +  sin0Ep  (2.47) 

Thus,  Eq.  (2.41)  reduces  to 

p'  =  [ee^  +  cos  0(1  —  ee^)  +  sin0E]p  (2.48) 

Erom  Eq.  (2.48)  it  is  apparent  that  p'  is  a  linear  transformation  of  p,  the 
transformation  being  given  by  the  expression  inside  the  brackets,  which  is  the 
rotation  matrix  Q  sought,  i.e., 

Q  =  ee^  +  cos  0(1  —  ee^)  +  sin0E  (2.49) 

A  special  case  arises  when  (p  =  n, 

Q  =  — 1  +  2ee^,  for  0  =  tt  (2.50) 

whence  it  is  apparent  that  Q  is  symmetric  if  0  =  tt.  Of  course,  Q  becomes 
symmetric  also  when  0  =  0,  but  this  is  a  rather  obvious  case,  leading  to  Q  =  1. 
Except  for  these  two  cases,  the  rotation  matrix  is  not  symmetric.  However,  under  no 
circumstance  does  the  rotation  matrix  become  skew- symmetric,  for  a  3  x  3  skew- 
symmetric  matrix  is  by  necessity  singular,  which  contradicts  the  property  of  proper 
orthogonal  matrices  of  Eq.  (2.21b). 

Now  one  more  representation  of  Q  in  terms  of  e  and  0  is  given.  Eor  a  fixed  axis 
of  rotation,  i.e.,  for  a  fixed  value  of  e,  the  rotation  matrix  Q  is  a  function  of  the  angle 
of  rotation  0,  only.  Thus,  the  series  expansion  of  Q  in  terms  of  0  is 

Q(<P)  =  Q(0)  +  Q'(0),p  +  2q"(0)./.2  +  •  ■  •  +  +  . . . 


(2.51) 
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where  the  superscript  {k)  stands  for  the  kih  derivative  of  Q  with  respect  to  0.  Now, 
from  the  definition  of  E,  one  can  readily  prove  the  relations  below: 

£(2*^+1)  =  (-l)*^E,  £2*^  =  (-l)*(l-ee2’)  (2.52) 

Furthermore,  using  Eqs.  (2.49)  and  (2.52),  one  can  readily  show  that 

=  E*  (2.53) 

with  E  defined  already  as  the  cross-product  matrix  of  e.  Moreover,  from  Eqs.  (2.51) 

and  (2.53),  Q(0)  can  be  expressed  as 

Q(0)  =  1  +  E0  +  ^£202  +  . . .  +  +  . . . 

^  •  Av  • 

whose  right-hand  side  is  nothing  but  the  exponential  of  E0,  i.e., 

Q(0)  =  (2.54) 


Equation  (2.54)  is  the  exponential  representation  of  the  rotation  matrix  in  terms  of  its 
natural  invariants,  e  and  0.  The  foregoing  parameters  are  termed  invariants  because 
they  are  independent  of  the  coordinate  axes  chosen  to  represent  the  rotation  under 
study.  The  adjective  natural  is  necessary  to  distinguish  them  from  other  invariants 
that  will  be  introduced  presently.  This  adjective  seems  suitable  because  the  said 
invariants  stem  naturally  from  Euler’s  Theorem. 

Now,  in  view  of  Eqs.  (2.52),  the  above  series  can  be  written  as 


QC-/-)  =  1  + 


-  —  (p^  +  —<p* - h  (-l)*'02<:  ^ 

2!^  AV  (2ky.  ’  ^ 


(1  —  ee^) 


+ 


1 


1 


(h  —  — (h  -|-  •  •  •  -|-  — 

^  3V  i2k  +  1)! 


(_1)*02J:  +  1 


E 


The  series  inside  the  first  pair  of  brackets  is  apparently  cos  0  —  1 ,  while  that  in  the 
second  pair  is  sin0.  We  have,  therefore,  an  alternative  representation  of  Q: 

Q  =  1  +  sin  0E  +  (1  —  cos  0)E^  (2.55) 


which  is  an  expected  result  in  view  of  the  Cayley-Hamilton  Theorem. 


The  Canonical  Forms  of  the  Rotation  Matrix 

The  rotation  matrix  takes  on  an  especially  simple  form  if  the  axis  of  rotation 
coincides  with  one  of  the  coordinate  axes.  For  example,  if  the  X  axis  is  parallel 
to  the  axis  of  rotation,  i.e.,  parallel  to  vector  e,  in  a  frame  that  we  will  label  X,  then, 
we  will  have 
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1 


00  0 


0  0  0 


[e]^’  =  0 
0 


[E];^  =  0  0-1  ,  [E"];^  =0-10 


0  1  0 


0  0-1 


In  the  A’-frame,  then, 


1  0  0 


[QIa' =  Ocos0— sin0 
_0  sin0  COS0 


(2.56a) 


Likewise,  if  we  define  the  coordinate  frames  y  and  Z  so  that  their  Y  and  Z  axes, 
respectively,  coincide  with  the  axis  of  rotation,  then 


cos(p  0  sin0 


[Q]y=  0  1  0 

_—  sin  (p  0  cos  0 


(2.56b) 


and 


cos  0  —  sin  0  0 
[Q]^:  =  sin0  COS0  0 


(2.56c) 


0  0  1 


The  representations  of  Eqs.  (2.56a-c)  can  be  called  the  X-,  Y and  Z -canonical 
forms  of  the  rotation  matrix.  In  many  instances,  a  rotation  matrix  cannot  be  derived 
directly  from  information  on  the  original  and  the  final  orientations  of  a  rigid 
body,  but  the  overall  motion  can  be  readily  decomposed  into  a  sequence  of  simple 
rotations  taking  the  above  canonical  forms.  An  application  of  canonical  forms  lies 
in  the  parameterization  of  rotations  by  means  of  Euler  angles,  consisting  of  three 
successive  rotations,  0,  0  and  0,  about  one  axis  of  a  coordinate  frame.  Euler  angles 
are  introduced  in  Exercise  2.19,  and  applications  thereof  are  given  in  Exercises  2.37, 
2.38  and  3.10. 


2.3.3  The  Linear  Invariants  o/a  3  x  3  Matrix 

Now  we  introduce  two  linear  invariants  of  3  x  3  matrices.  Given  any  3x3  matrix 
A,  its  Cartesian  decomposition,  the  counterpart  of  the  Cartesian  representation  of 
complex  numbers,  consists  of  the  sum  of  its  symmetric  part,  A^,  and  its  skew- 
symmetric  part,  A^^,  defined  as 


(2.57) 


2.3  Rigid-Body  Rotations 


45 


The  axial  vector  or  for  brevity,  the  vector  of  A,  is  the  vector  a  with  the  property 

a  X  V  =  A55V  (2.58) 

for  any  three-dimensional  vector  v.  The  trace  of  A  is  the  sum  of  the  eigenvalues  of 
A5,  which  are  real.  Since  no  coordinate  frame  is  involved  in  the  above  definitions, 
these  are  invariant.  When  calculating  these  invariants,  of  course,  a  particular 
coordinate  frame  must  be  used.  Let  us  assume  that  the  entries  of  matrix  A  in  a  certain 
coordinate  frame  are  given  by  the  array  of  real  numbers  atj^iov  i,j  =  1,2,3. 
Moreover,  let  a  have  components  a/ ,  for  /  =  1 , 2,  3,  in  the  same  frame.  The  above- 
defined  invariants  are  thus  calculated  as 


vect(A)  =  a  = 


1 

2 


^32  —  ^23 
^13  ~  ^31  ^ 

,^21  —  ^12_ 


tr(A)  =  ail  -\-  ^22  +  ^33 


(2.59) 


From  the  foregoing  definitions,  we  have  now 

Theorem  2.3.5.  The  vector  of  a  3x3  matrix  vanishes  if  and  only  if  it  is  symmetric, 
whereas  the  trace  of  an  n  x  n  matrix  vanishes  if  the  matrix  is  skew  symmetric. 

Other  useful  relations  are  given  below.  For  any  three-dimensional  vectors  a 
and  b. 


vect(ab^)  =  — a  x  b 


1 


(2.60) 


and 


tr(ab^)  =  a^b  (2.61) 

The  second  relation  is  quite  straightforward,  but  the  first  one  is  less  so;  a  proof  of 
the  first  relation  follows:  Let  w  denote  vect(ab^).  From  Definition  (2.58),  for  any 
three-dimensional  vector  v. 


w  X  V  =  Wv 


where  W  is  the  skew-symmetric  component  of  ab^,  namely. 


(2.62) 


(2.63) 


and  hence. 


Wv  =  w  X  V  = 


(a^v)b] 


(2.64) 
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Now,  let  us  compare  the  last  expression  with  the  double  cross  product^  (b  x  a)  x  v, 
namely, 


(b  X  a)  X  V  =  (b^v)a  —  (a^v)b  (2.65) 

from  which  it  becomes  apparent  that 

1 

w  =  2^  ^  ^  (2.66) 

thereby  proving  the  aforementioned  relation. 

Note  that  Theorem  2.3.5  states  a  necessary  and  sufficient  condition  for  the 
vanishing  of  the  vector  of  a  3  x  3  matrix,  but  only  a  sufficient  condition  for  the 
vanishing  of  the  trace  of  a.  n  x  n  matrix.  What  this  implies  is  that  the  trace  of  a 
n  X  n  matrix  can  vanish  without  the  matrix  being  necessarily  skew  symmetric,  but 
the  trace  of  a  skew- symmetric  matrix  necessarily  vanishes.  Also  note  that  whereas 
the  vector  of  a  matrix  is  defined  only  for  3  x  3  matrices,  the  trace  can  be  defined 
more  generally  fox  n  xn  matrices. 

In  some  applications,  the  cross-product  matrix  of  the  product  Ab  of  a  3  x  3  matrix 
A  by  a  vector  b  is  needed^ : 

CPM(Ab)  =  (BA)^  -  BA  +  tr(A)B  =  [tr(A)l  -  A^]  B  -  BA  (2.67) 

where  B  =  CPM(b).  The  reader  is  encouraged  to  verify  the  correctness  of  the  above 
relation  using  components.  Caveat:  a  component-free  proof  of  the  above  relation  is 
particularly  challenging. 


2.3.4  The  Linear  Invariants  of  a  Rotation 

From  the  invariant  representations  of  the  rotation  matrix,  Eqs.  (2.49)  and  (2.55),  it  is 
clear  that  the  first  two  terms  of  Q,  ee^  and  cos  0(1  —  ee^),  are  symmetric,  whereas 
the  third  one,  sin0E,  is  skew- symmetric.  Hence, 

vect(Q)  =  vect(sin0E)  =  sin0e  (2.68) 


whereas 

tr(Q)  =  tr[ee^  +  cos  0(1  —  ee^)]  =  e^e  +  cos  0(3  —  e^e)  =  1  +  2cos0  (2.69) 


^Popularly  known  as  the  triple  cross  product. 

^This  relation  was  derived  by  Ph.D.  candidate  Philippe  Cardou. 
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from  which  one  can  readily  solve  for  cos  0,  namely, 


COS0  = 


tr(Q)  -  1 
2 


(2.70) 


Henceforth,  the  vector  of  Q  will  be  denoted  by  q  and  its  components  in  a 
given  coordinate  frame  by  qi,  q2,  and  q^.  Moreover,  rather  than  using  tr(Q)  as 
the  other  linear  invariant,  qo  =  cos0  will  be  introduced  to  refer  to  the  linear 
invariants  of  the  rotation  matrix.  Hence,  the  rotation  matrix  is  fully  defined  hy  four 
scalar  parameters,  namely  which  will  be  conveniently  stored  in  the  four¬ 

dimensional  array  A ,  defined  as 


^  =  [qi,  qi,  qs,  qof 


(2.71) 


Note,  however,  that  the  four  components  of  A  are  not  independent,  for  they  obey 
the  relation 


q||^  +  =  sin^  0  +  cos^  0  =  1 


Thus,  Eq.  (2.72)  can  be  written  in  a  more  compact  form  as 


(2.72) 


(2.73) 


What  Eq.  (2.72)  states  has  a  straightforward  geometric  interpretation:  As  a  body 
rotates  about  a  fixed  point,  its  motion  can  be  described  in  a  four-dimensional  space 
by  the  motion  of  a  point  of  position  vector  A  that  moves  on  the  surface  of  the  unit 
sphere  centered  at  the  origin  of  the  said  space.  Alternatively,  one  can  conclude 
that,  as  a  rigid  body  rotates  about  a  fixed  point,  its  motion  can  be  described  in  a 
three-dimensional  space  by  the  motion  of  position  vector  q,  which  moves  within  the 
unit  solid  sphere  centered  at  the  origin  of  the  said  space.  Given  the  dependence  of 
the  four  components  of  vector  A ,  one  might  be  tempted  to  solve  for,  say,  qo  from 
Eq.  (2.72)  in  terms  of  the  remaining  components,  namely,  as 


^0  =  ±  f-(qi+ql  +  ql)  (2.74) 

This,  however,  is  not  a  good  idea  because  the  sign  ambiguity  of  Eq.  (2.74)  leaves 
angle  0  undefined,  for  qo  is  nothing  but  cos0.  Moreover,  the  three  components  of 
vector  q  alone,  i.e.,  sin0  e,  do  not  suffice  to  define  the  rotation  represented  by  Q. 
Indeed,  from  the  definition  of  q,  one  has 


sin0  =  zb||q||,  e  =  q/  sin0 


(2.75) 


from  which  it  is  clear  that  q  alone  does  not  suffice  to  define  the  rotation  under 
study,  since  it  leaves  angle  0  undefined.  Indeed,  the  vector  of  the  rotation  matrix 
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provides  no  information  about  cos(p.  Yet  another  representation  of  the  rotation 
matrix  is  displayed  below,  in  terms  of  its  linear  invariants,  that  is  readily  derived 
from  representations  (2.49)  and  (2.55),  namely. 


+  Q 


(2.76a) 


in  which  Q  is  the  cross-product  matrix  of  vector  q,  i.e.. 


9(q  X  x) 
9x 


for  any  vector  x. 

Note  that  by  virtue  of  Eq.  (2.72),  the  representation  of  Q  given  in  Eq.  (2.76a)  can 
be  expressed  alternatively  as 


T 

Q  =  ^ol  +  Q  +  (2.76b) 

1  +qo 


Erom  either  Eq.  (2.76a)  or  (2.76b)  it  is  apparent  that  linear  invariants  are  not  suitable 
to  represent  a  rotation  when  the  associated  angle  is  either  tt  or  close  to  it.  Note  that  a 
rotation  through  an  angle  0  about  an  axis  given  by  vector  e  is  identical  to  a  rotation 
through  an  angle  —(p  about  an  axis  given  by  vector  — e.  Hence,  changing  the  sign  of 
e  does  not  change  the  rotation  matrix,  provided  that  the  sign  of  0  is  also  changed. 
Henceforth,  we  will  choose  the  sign  of  the  components  of  e  so  that  sin  0  >  0,  which 
is  equivalent  to  assuming  that  0  <  0  <  tt.  Thus,  sin0  is  calculated  as  ||q||,  while 
cos  0  as  indicated  in  Eq.  (2.70).  Obviously,  e  is  simply  q  normalized,  i.e.,  q  divided 
by  its  Euclidean  norm. 


2.3.5  Examples 


The  examples  below  are  meant  to  stress  the  foregoing  ideas  on  rotation  invariants. 

Example  2.3.3.  If  [e] jr  =  [a/3/3,  —a/3/3,  a/3/3  in  a  given  coordinate  frame  T 
and  0  =  120°,  what  is  Q  in  Tl. 

Solution:  Erom  the  data. 


1 

COS0  = 


sin0  = 


V3 

2 


Moreover,  in  the  T  frame. 


II 

"  1  ■ 

"  1  -1  1  ■ 

-1 

-1  1  -1 

_  1  _ 

_  1  -1  1  _ 
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and  hence, 


[l-ee^^ 


2  1  -1 

1 

o 

1  2  1 

1  0  -1 

1 

K) 

1 _ 

1 

o 

_ 1 

Thus,  from  Eq.  (2.49), 


,  ,  1 

"  1  -1  1  ■ 

1 

"2  1  -r 

3 

"0  -1  -\ 

[Q]^  =  3 

-1  1  -1 
_  1  -1  1  _ 

~  6 

1  2  1 
_-l  1  2  _ 

+  6 

1  0  -1 

_1  1  0  _ 

i.e.. 


0-10 
0  0-1 
1  0  0 


Example  2.3.4.  The  matrix  representation  of  a  linear  transformation  Q  in  a  certain 
reference  frame  T  is  given  below.  Find  out  whether  the  said  transformation  is  a 
rigid-body  rotation.  If  it  is,  find  its  natural  invariants. 


[Q]t 


0  1  0 
0  0  1 
1  00 


Solution:  First  the  given  array  is  tested  for  orthogonality: 


[Q]j^[Q^  \t  = 

"0  1  o' 
00  1 

'0  0  r 
1  0  0 

— 

'1  0  o' 
0  1  0 

_1  0  0_ 

_0  1  o_ 

_0  0  1_ 

thereby  showing  that  the  said  array  is  indeed  orthogonal.  Thus,  the  linear  transfor¬ 
mation  could  represent  a  reflection  or  a  rotation.  In  order  to  decide  which  one  this 
represents,  the  determinant  of  the  foregoing  array  is  computed: 

det(Q)  =  +1 

which  makes  apparent  that  Q  indeed  represents  a  rigid-body  rotation.  Now,  its  nat¬ 
ural  invariants  are  computed.  The  unit  vector  e  can  be  computed  as  the  eigenvector 
of  Q  associated  with  the  eigenvalue  +1.  This  requires,  however,  finding  a  nontrivial 
solution  of  a  homogeneous  linear  system  of  three  equations  in  three  unknowns.  This 
is  not  difficult  to  do,  but  it  is  cumbersome  and  is  not  necessary.  In  order  to  find  e  and 
0,  it  is  recalled  that  vect(Q)  =  sin^  e,  which  is  readily  computed  with  differences 
only,  as  indicated  in  Eq.  (2.59),  namely. 
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[q]^  =  sin0  [e]^ 


1 

1 

1 


Under  the  assumption  that  sin  ^  >  0,  then, 


and  hence, 


V3 

2 


and 


V3 

3 


1 

1 

1 


(j)  =  60°  or  120° 

The  foregoing  ambiguity  is  resolved  by  the  trace  of  Q,  which  yields 


1+2  cos  (p  =  tr(Q)  =  0, 


1 

cosd)  =  — 
2 


The  negative  sign  of  cos  (p  indicates  that  (p  lies  in  the  second  quadrant — it  cannot  lie 
in  the  third  quadrant  because  of  our  assumption  about  the  sign  of  sin  (p — and  hence 


(p  =  120° 


Example  2.3.5.  A  coordinate  frame  X\,  Ti,  Zi  is  rotated  into  a  configuration 
X2,  T2,  Z2  in  such  a  way  that 

X2  =  -Yi,  F2  =  Zi,  Z2  =  -Xi 

Find  the  matrix  representation  of  the  rotation  in  Xi,  Ti,  Zi  coordinates.  From  this 
representation,  compute  the  direction  of  the  axis  and  the  angle  of  rotation. 

Solution:  Let  be  unit  vectors  parallel  to  Xi,  Ti,Zi,  respectively, 

12,  j2,  1^2  being  defined  correspondingly.  One  has 

L  = -ji,  j2  =  ki,  k2  = -ii 

and  hence,  from  Definition  2.2.1,  the  matrix  representation  [Q]i  of  the  rotation 
under  study  in  the  Xi,  Ti,  Zi  coordinate  frame  is  readily  derived: 


0  0-1 
-10  0 
0  1  0 


[Q]i  = 
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from  which  the  linear  invariants  follow,  namely, 


[q]i  =  [vect(Q)]i  =  sin0[e]i  =  ^ 


■  1  ■ 
-1 
-1 


cos(p  =  ^[tr(Q)  -l]  = 


Under  our  assumption  that  sin  ^  >  0,  we  obtain 


sin0  =  II  q 


[q]i 

[e]i  = 


V3 


sin0 


■  1 
-1 
-1 


From  the  foregoing  values  for  sin  <j)  and  cos  (j),  angle  (p  is  computed  uniquely  as 

(p  =  120° 


Example  2.3.6.  Show  that  the  matrix  P  given  in  Eq.  (2.4)  satisfies  properties  (2.1a). 


Solution:  First,  we  prove  idempotency,  i.e., 

=  (1  —  nn^)(l  —  nn^) 

=  1  —  2nn^  +  nn^nn^  =  1  —  nn^  =  P 


thereby  showing  that  P  is,  indeed,  idempotent.  Now  we  prove  that  n  is  an 
eigenvector  of  P  with  eigenvalue,  0  and  hence,  n  spans  the  null  space  of  P.  In  fact, 

rri  rri 

Pn  =  (1  —  nn  )n  =  n  —  nn  n  =  n  —  n  =  0 


thereby  completing  the  proof. 


Example  2.3.7.  The  representations  of  three  linear  transformations  in  a  given 
coordinate  frame  T  are  given  below: 


2  1  2 
-2  2  1 
-1  -2  2 


1 

3 


2  1  1 
1  2  -1 
1  -1  2 


1  2  2 
2  1  -2 
2  -2  1 


One  of  the  foregoing  matrices  is  an  orthogonal  projection,  one  is  a  reflection,  and 
one  is  a  rotation.  Identify  each  of  these  and  give  its  invariants. 
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Solution:  From  representations  (2.49)  and  (2.55),  it  is  clear  that  a  rotation  matrix 
is  symmetric  if  and  only  if  sin  ^  =  0.  This  means  that  a  rotation  matrix  cannot  be 
symmetric  unless  its  angle  of  rotation  is  either  0  or  tt,  i.e.,  unless  its  trace  is  either  3 
or  —1.  Since  [B]jr  and  [C]jr  are  symmetric,  they  cannot  be  rotations,  unless  their 
traces  take  the  foregoing  values.  Their  traces  are  thus  evaluated  below: 

tr(B)  =  2,  tr(C)  =  1 

which  thus  rules  out  the  foregoing  matrices  as  suitable  candidates  for  rotations. 
Thus,  A  is  the  only  candidate  left  for  proper  orthogonality,  its  suitability  being  tested 
below: 


[AA^b=  1 


■900' 

0  90 
00  9 


,  det(A)  =  +1 


and  hence,  A  indeed  represents  a  rotation.  Its  natural  invariants  are  next  computed: 


sincj)  [e]jr  =  [vect(A)]^  =  ^ 


-1' 

1 

-1 


COS0  =  ^[tr(A)  =  =  ^ 


We  assume,  as  usual,  that  sin^  >  0.  Then, 


\/3 

sincj)  =  ||vect(A)||  =  — ,  i.e.,  cj)  =  60 


Moreover, 


^  [vect(A)]jr  ^  Vs 
||vect(A)||  3 

Now,  one  matrix  of  B  and  C  is  an  orthogonal  projection  and  the  other  is  a  reflection. 
To  be  a  reflection,  a  matrix  has  to  be  orthogonal.  Hence,  each  matrix  is  tested  for 
orthogonality: 


-1 

1 

-1 


"6  3  3  ■ 

=  [b2]^  =  [B]^, 

"9  0  0" 

3  6-3 

0  90 

y 

_3  -3  6  _ 

y 

_0  0  9_ 

thereby  showing  that  C  is  orthogonal  and  B  is  not.  Furthermore,  det(C)  =  —  1, 
which  confirms  that  C  is  a  reflection.  Now,  if  B  is  a  projection,  it  is  bound  to  be 
singular  and  idempotent.  From  the  orthogonality  test  it  is  clear  that  it  is  idempotent. 
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Moreover,  one  can  readily  verify  that  det(B)  =  0,  and  hence  B  is  singular,  the  unit 
vector  [n]jr  =  [ni,  n2,  that  spans  its  null  space  being  determined  from  the 
general  form  of  projections,  Eq.  (2.1a),  whence, 

nn^  =  1  -  B 


Therefore,  if  a  solution  n  has  been  found,  then  — n  is  also  a  solution,  i.e.,  the  problem 
admits  two  solutions,  one  being  the  negative  of  the  other.  These  two  solutions  are 
found  below,  by  first  rewriting  the  above  system  of  equations  in  component  form: 


n\  nin2  nin^ 
nin2  n\  n2n3 


1 

"  1  -1  -r 

'X 

-1  1  1 

J 

_-i  1  1  _ 

Now,  from  the  diagonal  entries  of  the  above  matrices,  it  is  apparent  that  the  three 
components  of  n  have  identical  absolute  values,  i.e.,  V3/3.  Moreover,  from  the  off- 
diagonal  entries  of  the  same  matrices,  the  second  and  third  components  of  n  bear 
equal  signs,  but  we  cannot  tell  whether  positive  or  negative,  because  of  the  quadratic 
nature  of  the  problem  at  hand.  The  two  solutions  are  thus  obtained  as 


n  = 


1 

-1 

-1 


which  is  the  only  invariant  of  B. 

We  now  look  at  C,  which  is  a  reflection,  and  apparently,  bears  the  form 

C  =  1  -  2nn^ 


In  order  to  determine  n,  note  that 


or  in  component  form. 


1 

2 


(l-C) 


n\  nin2  niu^ 
nin2  n\  772^3 

77i773  772773  n\ 


1 

■  1  -1  -r 

'X 

-1  1  1 

_-i  1  1  _ 

which  is  identical  to  the  matrix  equation  derived  in  the  case  of  matrix  B.  Hence,  the 
solution  is  the  same,  i.e.. 


n  = 


1 

-1 

-1 


thereby  finding  the  invariant  sought. 
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Example  2.3.8.  The  vector  and  the  trace  of  a  rotation  matrix  Q,  in  a  certain 
reference  frame  T,  are  given  as 


[vect(Q)]^  = 


1 

2 


-1 

1 

-1 


tr(Q)  =  2 


Find  the  matrix  representation  of  Q  in  the  given  coordinate  frame  and  in  a  frame 
having  its  Z-axis  parallel  to  vect(Q). 

Solution:  We  shall  resort  to  Eq.  (2.76a)  to  determine  the  rotation  matrix  Q.  The 
quantities  involved  in  the  representation  of  Q  in  7^  are  readily  computed: 


1 

2 


0  1  1 
-1  0  1 
-1  -1  0 


from  which  Q  follows: 


[Q]t 


2  1  2 
-2  2  1 
-1  -2  2 


in  the  given  coordinate  frame.  Now,  let  Z  denote  a  coordinate  frame  whose  Z-axis 
is  parallel  to  q.  Hence, 


V3 

2 


"O' 

r  r  1  ^ 

-  [qq  h  =  T 

"0  0  0" 

r-n  V3 

0  -1 

0" 

0 

000 

1  0 

0 

_1_ 

_0  0  1_ 

2^ 

o 

o 
_ 1 

0_ 

which  readily  leads  to 


[Q]z  = 


1/2  -V3/2  0 

Z/2  1/2  0 
0  0  1 


and  is  in  the  Z-canonical  form. 

Example  2.3.9.  A  procedure  for  trajectory  planning  produced  a  matrix  representing 
a  rotation  for  a  certain  pick-and-place  operation,  as  shown  below: 


0.433  -0.500  z 
X  0.866  —0.433 
0.866  j  0.500 
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where  x,  y,  and  z  are  entries  that  are  unrecognizable  due  to  failures  in  the  printing 
hardware.  Knowing  that  Q  is  in  fact  a  rotation  matrix,  find  the  missing  entries. 

Solution:  Since  Q  is  a  rotation  matrix,  the  product  P  =  Q^Q  should  equal  the 
3x3  identity  matrix,  and  det(Q)  should  be  + 1 .  The  foregoing  product  is  computed 
first: 


0.437  +  0.433(x  -  z  -  1)  0.5(-y  +  z)  +  0.375 

*  0.937  +  ^2  0.866(x +  };)- 0.216 

*  *  1  + 


where  the  entries  below  the  diagonal  need  not  be  printed  because  the  matrix  is 
symmetric.  Upon  equating  the  diagonal  entries  of  the  foregoing  array  to  unity,  we 
obtain 


X  =  zbO.250,  J  =  0,  z  =  ±0.750 
while  the  vanishing  of  the  off-diagonal  entries  leads  to 

X  =  0.250,  J  =  0,  z  =  —0.750 
which  can  be  readily  verified  to  produce  det(Q)  =  ±  1 . 


2.3.6  The  Euler-Rodrigues  Parameters 


The  invariants  defined  so  far,  namely,  the  natural  and  the  linear  invariants  of  a 
rotation  matrix,  are  not  the  only  ones  that  are  used  in  kinematics.  Additionally,  one 
has  the  Euler  parameters,  or  Euler-Rodrigues  parameters,  as  Cheng  and  Gupta 
(1989)  propose  that  they  should  be  called,  represented  here  as  r  and  tq.  The  Euler- 
Rodrigues  parameters  are  defined  as 


r  = 


e, 


(2.77) 


One  can  readily  show  that  Q  takes  on  a  quite  simple  form  in  terms  of  the  Euler- 
Rodrigues  parameters,  namely. 


Q  =  (ro^  —  r  •  r)l  ±  2rr^  ±  2roR 


in  which  R  is  the  cross-product  matrix  of  r,  i.e.. 


9(r  X  x) 
9x 


(2.78) 


for  arbitrary  x. 
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Note  that  the  Euler-Rodrigues  parameters  appear  quadratically  in  the  rotation 
matrix.  Hence,  these  parameters  cannot  be  computed  with  simple  sums  and 
differences.  A  closer  inspection  of  Eq.  (2.76b)  reveals  that  the  linear  invariants 
appear  almost  linearly  in  the  rotation  matrix.  This  means  that  the  rotation  matrix, 
as  given  by  Eq.  (2.76b),  is  composed  of  two  types  of  terms,  namely,  linear  and 
rational.  Moreover,  the  rational  term  is  composed  of  a  quadratic  expression  in  the 
numerator  and  a  linear  expression  in  the  denominator,  the  ratio  thus  being  linear, 
which  explains  why  the  linear  invariants  can  be  obtained  by  sums  and  differences 
from  the  rotation  matrix. 

The  relationship  between  the  linear  invariants  and  the  Euler-Rodrigues  parame¬ 
ters  can  be  readily  derived,  namely. 


(2.79) 


Eurthermore,  note  that,  if  (p  =  Jt,  then  tq  =  0,  and  formulae  (2.79)  fail  to  produce 
r.  However,  from  Eq.  (2.77), 


Eor  (p  =  7t:  r  =  e,  ro  =  0 


(2.80) 


We  now  derive  invariant  relations  between  the  rotation  matrix  and  the  Euler- 
Rodrigues  parameters.  To  do  this,  we  resort  to  the  concept  of  matrix  square  root. 
As  a  matter  of  fact,  the  square  root  of  a  square  matrix  is  nothing  but  a  particular 
case  of  an  analytic  function  of  a  square  matrix,  discussed  in  connection  with 
Theorem  2.3.3  and  the  exponential  representation  of  the  rotation  matrix.  Indeed, 
the  square  root  of  a  square  matrix  is  an  analytic  function  of  that  matrix,  and  hence, 
admits  a  series  expansion  in  powers  of  the  matrix.  Moreover,  by  virtue  of  the 
Cayley-Hamilton  Theorem  (Theorem  2.3.3)  the  said  square  root  should  be,  for  a 
3x3  matrix,  a  linear  combination  of  the  identity  matrix  1,  the  matrix  itself,  and  its 
square,  the  coefficients  being  found  using  the  eigenvalues  of  the  matrix. 

Eurthermore,  from  the  geometric  meaning  of  a  rotation  through  the  angle  (p 
about  an  axis  parallel  to  the  unit  vector  e,  it  is  apparent  that  the  square  of  the 
matrix  representing  the  foregoing  rotation  is  itself  a  rotation  about  the  same  axis, 
but  through  the  angle  20.  By  the  same  token,  the  square  root  of  the  rotation  matrix 
is  again  a  rotation  matrix  about  the  same  axis,  but  through  an  angle  0/2.  Now, 
while  the  square  of  a  matrix  is  unique,  its  square  root  is  not.  This  fact  is  apparent 
for  diagonalizable  matrices,  whose  diagonal  entries  are  their  eigenvalues.  Each 
eigenvalue,  whether  positive  or  negative,  admits  two  square  roots,  and  hence,  a 
diagonalizable  n  xn  matrix  admits  as  many  square  roots  as  there  are  combinations 
of  the  two  possible  roots  of  individual  eigenvalues,  disregarding  rearrangements  of 
the  latter.  Such  a  number  is  2",  and  hence,  a  3  x  3  matrix  admits  eight  square  roots. 
Eor  example,  the  eight  square  roots  of  the  identity  3x3  matrix  are  displayed  below: 


2.3  Rigid-Body  Rotations 


57 


1  00 

0  10  , 

_0  0  1_ 

1 - 1 

O  O 

O  o 

o  o 

1 _ 1 

10  0' 
0-10  , 

0  0-1 

"-1  0  O' 

0  1  0 

0  0-1 

"-1  0  O' 

1  0  0 

0  1  0 

0-10 

9 

0  0  1 

o 

o 

-10  0 

-10  0 

0-10 

9 

0-10 

1 

o 

o 

1 _ 

1 

o 

o 

In  fact,  the  foregoing  result  can  be  extended  to  orthogonal  matrices  as  well  and, 
for  that  matter,  to  any  square  matrix  with  n  linearly  independent  eigenvectors.  That 
is,  an  ^  X  /I  orthogonal  matrix  admits  2"  square  roots.  However,  not  all  eight  square 
roots  of  a  3  X  3  orthogonal  matrix  are  orthogonal.  In  fact,  not  all  eight  square  roots  of 
a  3  X  3  proper  orthogonal  matrix  are  proper  orthogonal  either.  Of  these  square  roots, 
nevertheless,  there  is  one  that  is  proper  orthogonal,  the  one  representing  a  rotation  of 
0/2.  We  will  denote  this  particular  square  root  of  Q  by  VQ-  The  Euler-Rodrigues 
parameters  of  Q  can  thus  be  expressed  as  the  linear  invariants  of  namely, 

r  =  vect(v^),  ro  =  — \  (2.81) 

It  is  important  to  recognize  the  basic  differences  between  the  linear  invariants 
and  the  Euler-Rodrigues  parameters.  Whereas  the  former  can  be  readily  derived 
from  the  matrix  representation  of  the  rotation  involved  by  simple  additions  and 
subtractions,  the  latter  require  square  roots  and  entail  sign  ambiguities.  However, 
the  former  fail  to  produce  information  on  the  axis  of  rotation  whenever  the  angle  of 
rotation  is  tt,  whereas  the  latter  produce  that  information /6>r  any  value  of  the  angle 
of  rotation. 

The  Euler-Rodrigues  parameters  are  nothing  but  the  quaternions  invented  by 
Sir  William  Rowan  Hamilton  (1844)  in  an  extraordinary  moment  of  creativity  on 
Monday,  October  16,  1843,  as  “Hamilton,  accompanied  by  Lady  Hamilton,  was 
walking  along  the  Royal  Canal  in  Dublin  towards  the  Royal  Irish  Academy,  where 
Hamilton  was  to  preside  a  meeting”  (Altmann  1989). 

Moreover,  the  Euler-Rodrigues  parameters  should  not  be  confused  with  the 
Euler  angles,  which  are  not  invariant  and  hence,  admit  multiple  definitions.  The 
foregoing  means  that  no  single  set  of  Euler  angles  exists  for  a  given  rotation  matrix, 
the  said  angles  depending  on  how  the  rotation  is  decomposed  into  three  simpler 
rotations.  Eor  this  reason,  Euler  angles  will  not  be  stressed  here.  The  reader  is 
referred  to  Exercise  2.19  for  a  short  discussion  of  Euler  angles;  Synge  (1960) 
includes  a  classical  treatment,  while  Kane  et  al.  (1983)  provide  an  extensive 
discussion  of  the  same. 

Example  2.3.10.  Eind  the  Euler-Rodrigues  parameters  of  the  proper  orthogonal 
matrix  Q  given  as 

-12  2’ 

2-12 
2  2-1 
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Solution:  Since  the  given  matrix  is  symmetric,  its  angle  of  rotation  is  tt  and  its 
vector  linear  invariant  vanishes,  which  prevents  us  from  finding  the  direction  of 
the  axis  of  rotation  from  the  linear  invariants;  moreover,  expressions  (2.79)  do  not 
apply.  However,  we  can  use  Eq.  (2.50)  to  find  the  unit  vector  e  parallel  to  the  axis  of 
rotation,  i.e.. 


ee 


T 


^(i  +  Q) 


or  in  component  form. 


e\  6x62  eie^ 

6x62  ^2^3 

^1^3  ^2^3  ^3 


1 

3 


1  1  1 
1  1  1 
1  1  1 


A  simple  inspection  of  the  components  of  the  two  sides  of  the  above  equation  reveals 
that  all  three  components  of  e  are  identical  and  moreover,  of  the  same  sign,  but  we 
cannot  tell  which  sign  this  is.  Therefore, 


e  =  zb 


x/3 


1 

1 

1 


Moreover,  from  the  symmetry  of  Q,  we  know  that  (p  =  n,  and  hence. 


r  =  e  sm  —  =  ±  — 

2  /  3 


1' 

1 

1 


ro  =  cos  (  ^  I  =  0 


2.4  Composition  of  Reflections  and  Rotations 

As  pointed  out  in  Sect.  2.2,  reflections  occur  often  accompanied  by  rotations.  The 
effect  of  this  combination  is  that  the  rotation  destroys  the  two  properties  of  pure 
reflections,  symmetry  and  self-inversion,  as  defined  in  Sect.  2.2.  Indeed,  let  R  be 
a  pure  reflection,  taking  on  the  form  appearing  in  Eq.  (2.5),  and  Q  an  arbitrary 
rotation,  taking  on  the  form  of  Eq.  (2.49).  The  product  of  these  two  transformations, 
QR,  denoted  by  T,  is  apparently  neither  symmetric  nor  self-inverse,  as  the  reader 
can  readily  verify.  Likewise,  the  product  of  these  two  transformations  in  the  reverse 
order  is  neither  symmetric  nor  self-inverse. 

As  a  consequence  of  the  foregoing  discussion,  an  improper  orthogonal  trans¬ 
formation  that  is  not  symmetric  can  always  be  decomposed  into  the  product  of  a 
rotation  and  a  pure  reflection,  the  latter  being  symmetric  and  self-inverse.  Moreover, 
this  decomposition  can  take  on  the  form  of  any  of  the  two  possible  orderings  of  the 
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rotation  and  the  reflection.  Note,  however,  that  once  the  order  has  been  selected, 
the  decomposition  is  not  unique.  Indeed,  if  we  want  to  decompose  T  in  the  above 
paragraph  into  the  product  QR,  then  we  can  freely  choose  the  unit  normal  n  of  the 
plane  of  reflection  and  write 


R  =  1  -  2nn^ 


vector  n  then  being  found  from 


nn^  =  1(1 -R) 

Hence,  the  factor  Q  of  that  decomposition  is  obtained  as 

Q  =  TR“‘  =  TR  =  T  -  2(Tn)n^ 

where  use  has  been  made  of  the  self-inverse  property  of  R.  Any  other  selection  of 
vector  n  will  lead  to  a  different  decomposition  of  T. 

Example  2.4.1.  Join  the  palms  of  your  two  hands  in  the  position  adopted  by 
swimmers  when  preparing  for  plunging,  while  holding  a  sheet  of  paper  between 
them.  The  sheet  deflnes  a  plane  in  each  hand  that  we  will  call  the  hand  plane, 
its  unit  normal,  pointing  outside  of  the  hand,  being  called  the  hand  normal  and 
represented  as  vectors  and  for  the  right  and  left  hand,  respectively.  Moreover, 
let  Or  and  Or  denote  unit  vectors  pointing  in  the  direction  of  the  Anger  axes  of  each 
of  the  two  hands.  Thus,  in  the  swimmer  position  described  above,  Or  =  — and 
Ol  =  Now,  without  moving  your  right  hand,  let  the  left  hand  attain  a  position 
whereby  the  left-hand  normal  lies  at  right  angles  with  the  right-hand  normal,  the 
palm  pointing  downwards  and  the  Anger  axes  of  the  two  hands  remaining  parallel. 
Find  the  representation  of  the  transformation  carrying  the  right  hand  to  the  final 
configuration  of  the  left  hand,  in  terms  of  the  unit  vectors  Ur  and  Or  . 

Solution:  Let  us  regard  the  desired  transformation  T  as  the  product  of  a  rotation 
Qby  a  pure  reflection  R,  in  the  form  T  =  QR  Thus,  the  transformation  occurs  so 
that  the  reflection  takes  place  first,  then  the  rotation.  The  reflection  is  simply  that 
mapping  the  right  hand  into  the  left  hand,  and  hence,  the  reflection  plane  is  simply 
the  hand  plane,  i.e.. 


R  =  1  — 

Moreover,  the  left  hand  rotates  from  the  swimmer  position  about  an  axis  parallel 
to  the  Anger  axes  through  an  angle  of  90°  clockwise  from  your  viewpoint,  i.e.,  in 
the  positive  direction  of  vector  Or  .  Hence,  the  form  of  the  rotation  involved  can  be 
derived  readily  from  Eq.  (2.49)  and  the  above  information,  namely. 


Q  =  OrO^  +  Or 
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where  Or  is  the  cross-product  matrix  of  Or.  Hence,  upon  performing  the  product 
QR,  we  have 


T  =  o/jo^  +  20r  -  2{or  X 
which  is  the  transformation  sought. 


2.5  Coordinate  Transformations  and  Homogeneous 
Coordinates 

Crucial  to  robotics  is  the  unambiguous  description  of  the  geometric  relations  among 
the  various  bodies  in  the  environment  surrounding  a  robot.  These  relations  are 
established  by  means  of  coordinate  frames,  or  frames,  for  brevity,  attached  to  each 
rigid  body  in  the  scene,  including  the  robot  links.  The  origins  of  these  frames, 
moreover,  are  set  at  landmark  points  and  orientations  defined  by  key  geometric 
entities  like  lines  and  planes.  For  example,  in  Chap.  4  we  attach  two  frames  to  every 
moving  link  of  a  serial  robot,  with  origin  at  a  point  on  each  of  the  axis  of  the  two 
joints  coupling  this  link  with  its  two  neighbors.  Moreover,  the  Z-axis  of  each  frame 
is  defined,  according  to  the  Denavit-Hartenberg  notation,  introduced  in  that  chapter, 
along  each  joint  axis,  while  the  X-axis  of  the  frame  closer  to  the  base — termed  the 
fore  frame — is  defined  along  the  common  perpendicular  to  the  two  joint  axes.  The 
origin  of  the  same  frame  is  thus  defined  as  the  intersection  of  the  fore  axis  with  the 
common  perpendicular  to  the  two  axes.  This  section  is  devoted  to  the  study  of  the 
coordinate  transformations  of  vectors  when  these  are  represented  in  various  frames. 


2.5.1  Coordinate  Transformations  Between  Frames 
with  a  Common  Origin 

We  will  refer  to  two  coordinate  frames  in  this  section,  namely,  A  =  {X,  Y,  Z}  and 
B  =  {A,  y,  Z).  Moreover,  let  Q  be  the  rotation  carrying  A  into  B,  i.e., 

A  ^  B  (2.82) 

The  purpose  of  this  subsection  is  to  establish  the  relation  between  the  representa¬ 
tions  of  the  position  vector  of  a  point  P 'm  A  and  in  B,  denoted  by  [  p  and  [p]b, 
respectively.  Let 


[p]^  = 


(2.83) 
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Fig.  2.4  Coordinate  transformation:  (a)  coordinates  of  point  P  in  the  ^-frame;  and  (b)  relative 
orientation  of  frame  B  with  respect  to  A 


We  want  to  find  [  p  ]23  in  terms  of  [  p  and  Q,  when  the  latter  is  represented  in  either 
frame.  The  coordinate  transformation  can  best  be  understood  if  we  regard  point  P 
as  attached  to  frame  A,  as  if  it  were  a  point  of  a  box  with  sides  of  lengths  x,  y,  and 
z,  as  indicated  in  Fig.  2.4a.  Now,  frame  A  undergoes  a  rotation  Q  about  its  origin 
that  carries  it  into  a  new  attitude,  that  of  frame  B,  as  illustrated  in  Fig.  2.4b.  Point  P 
in  its  rotated  position  is  labeled  77,  of  position  vector  tt,  i.e., 

jt  =  Qp  (2.84) 


It  is  apparent  that  the  relative  position  of  point  P  with  respect  to  its  box  does  not 
change  under  the  foregoing  rotation,  and  hence. 


Moreover,  let 


(2.85) 


(2.86) 


The  relation  between  the  two  representations  of  the  position  vector  of  any  point  of 
the  three-dimensional  Euclidean  space  is  given  by 

Theorem  2.5.1.  The  representations  of  the  position  vector  it  of  any  point  in  two 
frames  A  and  B,  denoted  by  [ tt  and  [it]s,  respectively,  are  related  by 


[n]A  =  [Q].4[^]b 


(2.87) 
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Proof.  Let  us  write  Eq.  (2.84)  in  A: 

[^U  =  [QU[pU  (2.88) 

Now,  from  Fig.  2.4b  and  Eqs.  (2.83)  and  (2.85)  it  is  apparent  that 

[^]e  =  [pU  (2.89) 

Upon  substituting  Eq.  (2.89)  into  Eq.  (2.88),  we  obtain 

[jr].4  =  [Q].4[jr]t5  (2.90) 

q.e.d.  Moreover,  we  have 

Theorem  2.5.2.  The  representations  of  Q  carrying  A  into  B  in  these  two  frames 
are  identical,  i.e., 

[QU  =  [Q]t5  (2.91) 


Proof  Upon  substitution  of  Eq.  (2.84)  into  Eq.  (2.87),  we  obtain 


[Qp].4  =  [Q].4[Qp]b 


or 


[Q].4[P].4  =  [Q].4[Qp]b 

Now,  since  Q  is  orthogonal,  it  is  nonsingular,  and  hence,  [  Q  can  be  deleted  from 
the  foregoing  equation,  thus  leading  to 

[p].A  =  [Q]t5[p]t5  (2.92) 

However,  by  virtue  of  Theorem  2.5.1,  the  two  representations  of  p  observe  the 
relation 


[p].4  =  [Q].4[p]b  (2.93) 

the  theorem  being  proved  upon  equating  the  right-hand  sides  of  Eqs.  (2.92) 
and  (2.93). 

Note  that  the  foregoing  theorem  states  a  relation  valid  only  for  the  conditions 
stated  therein.  The  reader  should  not  conclude  from  this  result  that  rotation 
matrices  have  the  same  representations  in  every  frame.  This  point  is  stressed  in 
Example  2.5.1.  Furthermore,  we  have 
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Fig.  2.5  Coordinate  frames 
A  and  13  with  a  common 
origin 


Theorem  2.5.3.  The  inverse  relation  of  Theorem  2.5.1  is  given  by 

[jr]B  =  [Q^]B[jr]^  (2.94) 

Proof.  This  is  straightforward  in  light  of  the  two  foregoing  theorems,  and  is  left  to 
the  reader  as  an  exercise. 

Example  2.5.1.  Coordinate  frames  A  and  B  are  shown  in  Fig.  2.5.  Find  the 
representations  of  Q  rotating  A  into  B  in  these  two  frames  and  show  that  they  are 
identical.  Moreover,  if  [p]^  =  [1,  1,  1  ]^,  find  [p]b. 

Solution:  Let  i,  j,  and  k  be  unit  vectors  in  the  directions  of  the  X-,  Y-,  and  Z-axes, 
respectively;  unit  vectors  i,  y,  and  k  are  defined  likewise  as  parallel  to  the  A’-,  y-, 
and  Z-axes  of  Fig.  2.5.  Therefore, 

Qi  =  ^  =  -k,  Qj  =  y  = -i,  Qk  =  K=i 

Therefore,  using  Definition  2.2.1,  the  matrix  representation  of  Q  carrying  A  into 
B,m  A,  is  given  by 


[QU 


0-10 
0  0  1 
-10  0 


Now,  in  order  to  find  [  Q  ]23,  we  apply  Q  to  the  three  unit  vectors  of  B,  i,  y,  and  k. 
Thus,  for  L,  we  have 


0 

-1 

0 


=  Q(-k)  =  -Qk  = 


=  -}  =  -ic 
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Likewise, 


Qy  =  -L  Qk^  =  y 

again,  from  Definition  2.2.1,  we  have 


[Q]b 


0-10 
0  0  1 
-10  0 


=  [QU 


thereby  confirming  Theorem  2.5.2.  Note  that  the  representation  of  this  matrix  in  any 
other  coordinate  frame  would  be  different.  For  example,  if  we  represent  this  matrix 
in  a  frame  whose  X-axis  is  directed  along  the  axis  of  rotation  of  Q,  then  we  end  up 
with  the  X-canonical  representation  of  Q,  namely. 


1  0  0 


[QIa-  = 


0  cos  (p  —  sin  p 
0  sinp  cos(p 


with  the  angle  of  rotation  (p  being  readily  computed  as  ^  =  120°,  which  thus  yields 


1  0  0 
0  -1/2  -\/3/2 
0  x/3/2  -1/2 


Apparently,  the  entries  of  [Q]^/  are  different  from  those  of  [Q]^  and  [Q]23  found 
above. 

Now,  from  Eq.  (2.94), 


"  0  0  -r 

"r 

"-r 

-10  0 

1 

— 

-1 

_  0  1  0  _ 

_i_ 

_  1  _ 

a  result  that  can  be  readily  verified  by  inspection. 


2.5.2  Coordinate  Transformation  with  Origin  Shift 

Now,  if  the  coordinate  origins  do  not  coincide,  let  b  be  the  position  vector,  in 
A,  of  O,  the  origin  of  B,  as  shown  in  Fig.  2.6.  The  corresponding  coordinate 
transformation  from  A  to  B,  the  counterpart  of  Theorem  2.5.1,  is  given  below. 
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Theorem  2.5.4.  The  representations  of  the  position  vector  ^  of  a  point  P  of  the 
Euclidean  three-dimensional  space  in  two  frames  A  and  B  are  related  by 

[p]yt  =  [b]yt  +  [Q]^[^]e  (2.95a) 

Me  =  [Q^]B([-b]yt  +  [p]yt)  (2.95b) 

with  b  defined  as  the  vector  directed  from  the  origin  of  A  to  that  of  B,  and  it  the 
vector  directed  from  the  origin  of  B  to  P,  as  depicted  in  Fig.  2.6. 

Proof.  We  have,  from  Fig.  2.6,  in  any  coordinate  frame, 

p  =  b  +  TT  (2.96) 

If  we  express  the  above  equation  in  the  ^-frame,  we  obtain 

[P]yl  =  [b].4  +  [jt]A 

where  it  is  assumed  to  be  readily  available  in  B,  and  so  the  foregoing  equation  must 
be  expressed  as 


[p].4  =  [b].4  +  [Q].4[^]B  (2.97) 

which  thus  proves  Eq.  (2.95a).  To  prove  Eq.  (2.95b),  we  simply  solve  Eq.  (2.96)  for 
7t  and  apply  Eq.  (2.94)  to  the  equation  thus  resulting,  which  readily  leads  to  the 
desired  relation. 

Notice  the  geometric  interpretation  of  the  second  term  in  the  right-hand  side  of 
Eq.  (2.97):  this  term  represents,  in  frame  A,  the  position  vector  of  a  point  P\  whose 
image  under  Q  is  tt  . 
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Example  2.5.2.  If[b]^  =  [— 1,— 1,— 1]^  and  A  and  B  have  the  relative 

orientations  given  in  Example  2.5.1,  find  the  position  vector,  in  S,  of  a  point  P 
of  position  vector  [  p  given  as  in  the  same  example. 

Solution:  What  we  obviously  need  is  [  it  which  is  given  in  Eq.  (2.95b).  We  thus 
compute  first  the  sum  inside  the  parentheses  of  that  equation,  i.e.. 


[-^U  +  [p]^ 


2 

2 

2 


We  need  further  [QHb,  which  can  be  readily  derived  from  [Q]b.  We  do  not  have 
as  yet  this  matrix,  but  we  have  which  is  identical  to  [QHb  by  virtue  of 

Theorem  2.5.2.  Therefore, 


"  0  0  -r 

1 

(N 

1 _ 

(N 

1 

1 _ 

-10  0 

2 

— 

(N 

1 

_  0  1  0  _ 

1 

(N 

_ 1 

1 

(N 

_ 1 

a  result  that  the  reader  is  invited  to  verify  by  inspection. 


2.5.3  Homogeneous  Coordinates 


The  general  coordinate  transformation,  involving  a  shift  of  the  origin,  is  not  linear 
in  the  sense  of  the  definition  given  in  Subsection  2.2,  as  can  be  readily  realized  by 
virtue  of  the  nonhomo geneous  term  involved,  i.e.,  the  first  term  of  the  right-hand  side 
of  Eq.  (2.95a),  which  is  independent  of  p.  Such  a  transformation,  nevertheless,  can 
be  represented  in  homogeneous  form  if  homogeneous  coordinates  are  introduced. 
These  are  defined  below:  Let  [p]A/f  be  the  coordinate  array  of  sl  finite  point  P  in 
reference  frame  A4 .  What  we  mean  by  a  finite  point  is  one  whose  coordinates  are 
all  finite.  We  are  thus  assuming  that  the  point  P  at  hand  is  not  at  infinity,  points  at 
infinity  being  introduced  presently.  The  homogeneous  coordinates  of  P  are  those  in 
the  four-dimensional  array  {pIm^  defined  as 


{v}m  = 


[p]a^ 

1 


(2.98) 


The  affine  transformation  of  Eq.  (2.95a)  can  now  be  rewritten  in  homogeneous- 
coordinate  form  as 


{p}yt  = 


where  {T}^  is  defined  as  a  4  x  4  array,  namely. 


[Q].4  [b].4 
0^  1 


(2.99) 


(2.100) 
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The  inverse  transformation  of  that  defined  in  Eq.  (2.100)  is  derived  from  Eqs.  (2.95a 
and  b),  i.e., 


{T-‘}b 


[Q^  ]b  [-l>]e 
0^  1 


(2.101) 


Now,  {  T  ^  can  be  readily  derived  from  the  above  expression,  upon  applica¬ 
tion  of  Theorems  2.5.1  and  2.5.2,  which  leads  to 


{ T-‘ 


[Q^].4  -[Q]yt[b]B 
0^  1 


(2.102) 


Eurthermore,  homogeneous  transformations  can  be  concatenated.  Indeed,  let 
fork  =  /  —  1 ,  / ,  /  + 1 ,  denote  three  coordinate  frames,  with  origins  at  Ok .  Moreover, 
let  Q/_i  be  the  rotation  carrying  into  an  orientation  coinciding  with  that  of  Ti . 
If  a  similar  definition  for  Q/  is  adopted,  then  Q/  denotes  the  rotation  carrying  Ti 
into  an  orientation  coinciding  with  that  of  Eirst,  the  case  in  which  all  three 


origins  coincide  is  considered.  Clearly, 

[p]i  =  [Qr-i]<-i[p]<-i  (2.103a) 

[p];+i  =  [Qf  Mp]<  =  [Qf  MQr-iL-i[PL-i  (2.103b) 

the  inverse  relations  of  those  appearing  in  Eqs.  (2.103a  and  b)  being 

[p]i-i  =  [Qi-i]i-i[p]i  (2.104a) 

[p];-!  =  [Q/-i](-i[Q!  ]«[p]!+i  (2.104b) 

- > 


If  now  the  origins  do  not  coincide,  let  a,_i  and  a,  denote  the  vectors  Oi-iOi 

and  Oi  0,+i,  respectively.  The  transformations  and  {T/}/  thus  arising  are 

obviously 


[Q/— i]/— 1  i]/— 1 

0^  1 


[Q<];  [a/],- 

0^  1 


(2.105) 


whereas  their  inverse  transformations  are 


0^  1 


(2.106a) 


[  Qf  ];+i  [Qf  ]i+i  [  -a/  ]i 

0^  1 


(2.106b) 
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The  coordinate  transformations  involved  are  derived  upon  simply  replacing  Q;  _i 


and  Qi  with  T/_i  and  T/,  respectively,  in  Eqs.  (2.104a  and  b),  i.e., 

(2.107a) 

{p},_i  =  {T,_i},_i{T,},{p},+i  (2.107b) 

the  corresponding  inverse  transformations  being 

{p},-  =  {T-\},_i{p},_i  (2.108a) 

{p}i+i  =  {T-i},{p}/  =  {T-i},{T-\}i-i{p}i-i  (2.108b) 


which  are  the  counterpart  transformations  of  Eqs.  (2.103a  and  b)  for  the  case  of  no 
rigin  shift. 

Now,  if  P  lies  at  infinity,  we  can  express  its  homogeneous  coordinates  in  a 
simpler  form.  To  this  end,  we  rewrite  expression  (2.98)  in  the  form 


[e]A4 

.1/IIPl 


and  hence. 


lim  {p}yn  = 

pII^oo 


(lim 

IIpII^cx) 


[e]A^ 

.1/IIPll 


or 


lim  {p}a^  = 

PII^CX) 


[e]A4 

0 


We  now  define  the  homogeneous  coordinates  of  a  point  P  lying  at  infinity  as  the 
four-dimensional  array  appearing  in  the  foregoing  expression,  i.e.. 


{  Pcx)  }m 


(2.109) 


which  means  that  a  point  at  infinity,  in  homogeneous  coordinates,  has  only  a 
direction,  given  by  the  unit  vector  e,  but  an  undefined  location.  When  working  with 
objects  within  the  atmosphere  of  the  Earth,  for  example,  stars  can  be  regarded  as 
lying  at  infinity,  and  hence,  their  location  is  completely  specified  simply  by  their 
longitude  and  latitude,  which  suffice  to  define  the  direction  cosines  of  a  unit  vector 
in  spherical  coordinates. 

On  the  other  hand,  a  rotation  matrix  can  be  regarded  as  composed  of  three 
columns,  each  representing  a  unit  vector,  e.g.. 


Q  =  [ei  62  63] 
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where  the  triad  {  is  orthonormal.  We  can  thus  represent  {  T  of  Eq.  (2.100) 
in  the  form 


{TU 


ei  62  63  b 
0  0  0  1 


(2.110) 


thereby  concluding  that  the  columns  of  the  4x4  matrix  T  represent  the  homoge¬ 
neous  coordinates  of  a  set  of  corresponding  points,  the  first  three  of  which  lie  at 
infinity. 

Example  2.5.3.  An  ellipsoid  is  centered  at  a  point  Os  of  position  vector  b,  its 
three  axes  X,  y,  and  Z  defining  a  coordinate  frame  B.  Moreover,  its  semiaxes 
have  lengths  a  =  I,  b  =  2,  and  c  =  3,  the  coordinates  of  Os  in  a 
coordinate  frame  A  being  [b]^  =  [1,  2,  3]^.  Additionally,  the  direction  cosines 
of  A’  are  (0.933,  0.067,  —0.354),  whereas  y  is  perpendicular  to  b  and  to  the 
unit  vector  u  that  is  parallel  to  the  A-axis.  Find  the  equation  of  the  ellipsoid 
in  A.  (This  example  has  relevance  in  collision- avoidance  algorithms,  some  of 
which  approximate  manipulator  links  as  ellipsoids,  thereby  easing  tremendously 
the  computational  requirements.) 

Solution:  Let  u,  v,  and  w  be  unit  vectors  parallel  to  the  A-,  y-,  and  Z-axes, 
respectively.  Then, 


Wa 


0.933 

0.067 

-0.354 


u  X  b 
u  X  b 


w  =  u  X  V 


and  hence. 


"  0.243' 

'-0.266' 

-0.843 

.  [W]yt  = 

-0.535 

_  0.481  _ 

_-0.803_ 

from  which  the  rotation  matrix  Q,  rotating  the  axes  of  A  into  orientations  coinciding 
with  those  of  B,  can  be  readily  represented  in  A,  or  in  B  for  that  matter,  as 


[Q]^  =  [u,  V,  w]^  = 


0.933  0.243  -0.266 

0.067  -0.843  -0.535 
-0.354  0.481  -0.803 


On  the  other  hand,  if  the  coordinates  of  a  point  P  in  A  and  S  are  [p]^  = 
[pi,  Pi,  PiY  [^]b  =  [  ^1,  7r2,  ttb  ]^,  respectively,  then  the  equation  of  the 
ellipsoid  in  S  is,  apparently. 


JX 1  JX  JX  T 

—  H — ^  H — ^ 

12  ^  22  32 
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Now,  what  is  needed  in  order  to  derive  the  equation  of  the  ellipsoid  in  A  is  simply 
a  relation  between  the  coordinates  of  F  in  13  and  those  in  A.  These  coordinates  are 
related  by  Eq.  (2.95b),  which  requires  [Q^  while  we  have  [Q]^.  Nevertheless, 
by  virtue  of  Theorem  2.5.2 


[Q^]b  =  [Q^]a  = 


0.933  0.067  -0.354 

0.243  -0.843  0.481 
-0.266  -0.535  -0.803 


Hence, 


0.933  0.067  -0.354 

0.243  -0.843  0.481 
-0.266  -0.535  -0.803 


( 

~-r 

Pi 

-2 

+ 

P2 

\ 

_-3_ 

.P?>. 

Therefore, 


TTi  =  0.933/>i  +  0.067/>2  —  0.354/>3  —  0.005 
7t2  =  0.243/>i  —  0.843/>2  +  OASlp^ 

7t3  =  — 0.266/>i  —  0.535p2  —  0.803/?3  +  3.745 


Substitution  of  the  foregoing  relations  into  the  ellipsoid  equation  in  B  leads  to 


A:  32.1521  pi^  +  lJ0235p2^  +  9.112S6p3^  -  8.30524/?i  -  16.0527/12 

— 23.9304/>3  +  9.32655pip2  +  9.02784/>2i^3  —  19.9676/>i/>3  +  20.101  =  0 


which  is  the  equation  sought,  as  obtained  using  computer  algebra. 


2.6  Similarity  Transformations 

Transformations  of  the  position  vector  of  points  under  a  change  of  coordinate  frame 
involving  both  a  translation  of  the  origin  and  a  rotation  of  the  coordinate  axes 
was  the  main  subject  of  Sect.  2.5.  In  this  section,  we  study  the  transformations  of 
components  of  vectors  other  than  the  position  vector,  while  extending  the  concept 
to  the  transformation  of  matrix  entries.  How  these  transformations  take  place  is  the 
subject  of  this  section. 

What  is  involved  in  the  present  discussion  is  a  change  of  basis  of  the  associated 
vector  spaces,  and  hence,  this  is  not  limited  to  three-dimensional  vector  spaces. 
That  is,  w -dimensional  vector  spaces  will  be  studied  in  this  section.  Moreover,  only 
isomorphisms,  i.e.,  transformations  L  of  the  -dimensional  vector  space  V  onto 
itself  will  be  considered.  Let  A  =  {a^ and  B  =  {b, be  two  different  bases  of 
the  same  space  V.  Hence,  any  vector  v  of  V  can  be  expressed  in  either  of  two  ways, 
namely. 
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V  =  ttiai  +  ^2^2  +  •  •  •  +  (2.111) 

V  =  ^ibi  +  ^2^2  H“  •  •  •  H“  (2.112) 

from  which  two  representations  of  v  are  readily  derived,  namely, 


ai 

'isr 

[v]^  = 

0t2 

r  [v]s  = 

^2 

(2.113) 

In  order  to  find  the  representation  of  the  linear  transformation  that  relates  the 
two  bases,  let  A  be  the  linear  transformation  that  carries  the  set  A  into  the  set  B, 
i.e..  A:  A  ^  B. 

Furthermore,  let  the  two  foregoing  bases  be  related  by 

hj  =  aijSii  +  a2jSi2  H - h  <2„ya„,  j  =  1, . . .  ,^  (2.114) 

Hence,  the  representation  sought,  [A]^ — in  the  basis  A —  is,  according  to 
Definition  2.2.1, 


an  ai2  •  •  •  ain 


[AU  = 


^21  ^22  •  •  •  (^2n 


(2.115) 


Jlnl  tln2  '  "  d 


nnS 


Now,  in  order  to  find  the  relationship  between  the  two  representations  of 
Eq.  (2.113),  Eq.  (2.114)  is  substituted  into  Eq.  (2.112),  which  yields 

V  =  Pi(an^i  +  a2isi2  H - h  <3„ia„) 

+  ^2(^12^!  +  ^22^2  +  •  •  •  +  dfi2^n) 


+  ^nidln^l  +  ^2«^2  +  *  *  *  +  d^n^n) 


(2.116) 


This  can  be  rearranged  to  yield 


V  =  (dnPl  +  di2P2  H - h  dinPn)^l 

+  (d2lPl  +  d22^2  +  •  •  •  +  d2nPn)^2 


+  (dfilPl  +  dyi2^2  +  •  •  •  +  dfinPn)^n 


(2.117) 
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2  Mathematical  Background 


Comparing  Eq.  (2.1 17)  with  Eq.  (2.1 1 1),  one  readily  derives 

[v]^  =  [A]^[v]s  (2.118) 

with  [A]^  introduced  in  Eq.  (2.115).  Equation  (2.118)  is  the  relation,  sought,  its 
inverse  being 

[v]b  =  [A“‘]^[v]^  (2.119) 

Next,  let  L  have  the  representation  in  A  given  below: 


[LU 


^11  hi  •  •  •  hn 
hi  hi  •  •  •  hn 


(2.120) 


Now  we  aim  to  find  the  relationship  between  [ L  and  [  L ]23.  To  this  end,  let  w  be 
the  image  of  v  under  L,  i.e.. 


Lv  =  w  (2.121) 

which  can  be  expressed  in  terms  of  either  ^  or  S  as 

[L]^[v]^  =  [w]^  (2.122) 

[L]^[v]^  =  [w]^  (2.123) 

Eurther,  since  L  is  an  isomorphism  by  hypothesis,  w  of  Eq.  (2.121)  lies  in  the  same 
space  V  as  v.  Hence,  similar  to  Eq.  (2.118), 

[w]^  =  [A]^[w]^  (2.124) 

Now,  substitution  of  Eqs.  (2.118)  and  (2.124)  into  Eq.  (2.122)  yields 

[A]^[w]^  =  [L]^[A]^[v]^  (2.125) 

which  can  be  readily  rearranged  in  the  form 

[w]b  =  [A“']^[L]^[A]^[v]b  (2.126) 


Comparing  Eq.  (2.123)  with  Eq.  (2.126)  readily  leads  to 

[L]b  =  [  A“^  ]^[L]^[  A]^ 


(2.127) 
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which  upon  rearrangement,  becomes 

[LU  =  [A]^[L]b[A-1U  (2.128) 

Now,  paraphrasing  Theorems  2.5.2  and  2.5.4,  we  can  state 

Theorem  2.6.1.  The  representations  of  A  carrying  A  into  B  in  these  two  frames 
are  identical,  i.e., 


[AU  =  [A]^  (2.129) 

Proof  Substitute  L  for  A  in  Eq.  (2.127)  to  obtain  the  above  relation,  q.e.d. 

Relations  (2.118),  (2.119),  (2.127),  and  (2.128)  constitute  what  are  called 
similarity  transformations.  These  are  important  because  they  preserve  invariant 
quantities  such  as  the  eigenvalues  and  eigenvectors  of  matrices,  the  magnitudes  of 
vectors,  the  angles  between  vectors,  and  so  on.  Indeed,  one  has: 

Theorem  2.6.2.  The  characteristic  polynomial  of  a  given  n  x  n  matrix  remains 
unchanged  under  a  similarity  transformation.  Moreover,  the  eigenvalues  of  two 
matrix  representations  of  the  same  n  x  n  linear  transformation  are  identical,  and 
if[^]B  Is  an  eigenvector  of[h]B,  then  under  the  similarity  transformation  (2.128), 
the  corresponding  eigenvector  of  [  L]^  is  [e]^  =  [A]^[e]23. 

Proof.  From  Eq.  (2. 11),  the  characteristic  polynomial  of  [ L  ]23  is 

i^(A)  =  det(A[l]^-[L]^)  (2.130) 


which  can  be  rewritten  as 

/*(A)  =  det(A[A  A]^  —  [ A  ^]^[L]^[A]^) 

=  det([A“^]^(A[l]^  -  [L]^)[A]^) 

=  det([A“^]^)det(A[l]^  -  [L]^)det([  A]^) 


But 


det([A  ^]^)det([A]^)  =  1 

and  hence,  the  characteristic  polynomial  of  [  L  is  identical  to  that  of  [  L  ]23.  Since 
both  representations  have  the  same  characteristic  polynomial,  they  have  the  same 
eigenvalues.  Now,  if  [e]23  is  an  eigenvector  of  [L]23  associated  with  the  eigenvalue 
A,  then 


[^]b[^]b  =  A[e]23 
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Next,  Eq.  (2.127)  is  substituted  into  the  foregoing  equation,  which  thus  leads  to 

[A“M^[L]^[A]^[e]23  =  A[e]23 

Upon  rearrangement,  this  equation  becomes 

[L]^[A]^[e]23  =  A[A]^[e]23  (2.131) 

whence  it  is  apparent  that  [  A  ]^[  e ]23  is  an  eigenvector  of  [  L  associated  with  the 
eigenvalue  A,  q.e.d. 

Theorem  2.6.3.  ^  [L]^  and  [L,]b  cire  related  by  the  similarity  transformation 
(2.127),  then 

[L'^]b  =  [A-'U[L'^]^[A]^  (2.132) 


for  any  integer  k. 

Proof  This  is  done  by  induction.  For  k  =  2,  one  has 

M^[L]^[A]^[A  ^]^[L]^[A]^ 

=  [A-M^[L^U[AU 

Now,  assume  that  the  proposed  relation  holds  for  A  =  n.  Then, 

[L^+Mg  ^  [a-M.4[l'’U[au[a-M.4[l]x[a]^ 

=  [a-iu[l'’+iu[au 

i.e.,  the  relation  holds  for  /:  =  +  1  as  well,  thereby  completing  the  proof. 

Theorem  2.6.4.  The  trace  of  a  n  x  n  matrix  does  not  change  under  a  similarity 
transformation. 

Proof.  A  preliminary  relation  will  be  needed:  Let  [A],  [B]  and  [C]  be  three 
different  n  xn  matrix  arrays,  in  a  given  reference  frame,  that  need  not  be  indicated 
with  any  subscript.  Moreover,  let  aij ,  bij ,  and  Cij  be  the  components  of  the  said 
arrays,  with  indices  ranging  from  lion.  Hence,  using  standard  index  notation, 

tr([A]  [B]  [C])  =  UijbjkCki  =  bjkCuaij  =  tr([B]  [C]  [A])  (2.133) 

Taking  the  trace  of  both  sides  of  Eq.  (2.127)  and  applying  the  foregoing  result 
produces 

tr([L]B)  =  tr([A“‘]^[L]^[A]^)  =  tr([ A]^[ A“‘ ]^[L]^)  =  tr([L]^) 

(2.134) 

thereby  proving  that  the  trace  remains  unchanged  under  a  similarity  transformation. 

Example  2.6.1.  We  consider  the  equilateral  triangle  sketched  in  Fig.  2.7,  of  side 
length  equal  to  2,  with  vertices  Pi,  P2,  and  P3,  and  coordinate  frames  A  and  B  of 
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Fig.  2.7  Two  coordinate 
frames  used  to  represent  the 
position  vectors  of  the  comers 
of  an  equilateral  triangle 


A  r 


axes  X,  Y  and  X' ,  Y',  respectively,  both  with  origin  at  the  centroid  of  the  triangle. 
Let  P  be  a  2  X  2  matrix  defined  by 

p  =  [pi  P2] 

with  p,  denoting  the  position  vector  of  Pi  in  a  given  coordinate  frame.  Show  that 
matrix  P  does  not  obey  a  similarity  transformation  upon  a  change  of  frame,  and 
compute  its  trace  in  frames  A  and  B  to  make  it  apparent  that  this  matrix  does  not 
comply  with  the  conditions  of  Theorem  2.6.4. 

Solution:  From  the  figure  it  is  apparent  that 


1  0 
-V3/3  2x/3/3 


0  1 
-2x/3/3  x/3/3 


Apparently, 


]a)  —  1  + 


2V3 


tr([P]B) 


V3 

3 


The  reason  why  the  trace  of  this  matrix  did  not  remain  unchanged  under  a  coordinate 
transformation  is  that  the  matrix  does  not  obey  a  similarity  transformation  under  a 
change  of  coordinates.  Indeed,  vectors  p/  change  as 


[p/]^  =  [Q]^[p/]s 


under  a  change  of  coordinates  from  B  to  A,  with  Q  denoting  the  rotation  carrying 
A  into  B.  Hence, 


[PU  =  [QU[P]s 
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which  is  different  from  the  similarity  transformation  of  Eq.  (2.128).  However,  if  we 
now  define 


R  =  PP^ 


then 


[RU 


1  -V3/3 

-v^/3  5/3 


1  V3/3 

v^/3  5/3 


and  hence, 


tr([R]^)  =  tr([R]B)  =  ^ 

thereby  showing  that  the  trace  of  R  does  not  change  under  a  change  of  frame. 
In  order  to  verify  whether  matrix  R  complies  with  the  conditions  of  Theorem  2.6.4, 
we  notice  that,  under  a  change  of  frame,  matrix  R  changes  as 

[R]^  =  [PP^^  =  [[QU[p]b([QU[p]8)^  =  [QU[pp^]b[Q^U] 

which  is  indeed  a  similarity  transformation. 


2.7  Invariance  Concepts 

From  Example  2.6.1  it  is  apparent  that  certain  properties,  like  the  trace  of  certain 
square  matrices,  do  not  change  under  a  coordinate  transformation.  For  this  reason, 
a  matrix  like  R  of  that  example  is  said  to  he  frame -invariant,  or  simply  invariant, 
whereas  matrix  P  of  the  same  example  is  not.  In  this  section,  we  formally  define  the 
concept  of  invariance  and  highlight  its  applications  and  its  role  in  robotics.  Let  a 
scalar,  a  vector,  and  a  matrix  function  of  the  position  vector  p  be  denoted  by  / (p), 
f(p)  and  F(p),  respectively.  The  representations  of  f(p)  in  two  different  coordinate 
frames,  labelled  A  and  B,  will  be  indicated  as  [f(p)]^  and  [f(p)]23,  respectively, 
with  a  similar  notation  for  the  representations  of  F(p).  Moreover,  let  the  two  frames 
differ  both  in  the  location  of  their  origins  and  in  their  orientations.  Additionally, 
let  the  proper  orthogonal  matrix  [Q]^  denote  the  rotation  of  coordinate  frame  A 
into  B.  Then,  the  scalar  function  / (p)  is  said  to  be  frame  invariant,  or  invariant  for 
brevity,  if 


/([p]s)  =  /([p]^) 


Moreover,  the  vector  quantity  f  is  said  to  be  invariant  if 


(2.135) 


[f|^  = 


(2.136) 
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and  finally,  the  matrix  quantity  F  is  said  to  be  invariant  if 


WU  =  [Q].4[F]b[Q^].4 


(2.137) 


Thus,  the  difference  in  origin  location  becomes  irrelevant  in  this  context,  and 
hence,  will  no  longer  be  considered.  From  the  foregoing  discussion,  it  is  clear  that 
the  same  vector  quantity  has  different  components  in  different  coordinate  frames; 
moreover,  the  same  matrix  quantity  has  different  entries  in  different  coordinate 
frames.  However,  certain  scalar  quantities  associated  with  vectors,  e.g.,  the  inner 
product,  and  matrices,  e.g.,  the  matrix  moments,  to  be  defined  presently,  remain 
unchanged  under  a  change  of  frame.  Additionally,  such  vector  operations  as  the 
cross  product  of  two  vectors  are  invariant.  In  fact,  the  scalar  product  of  two  vectors 
a  and  b  remains  unchanged  under  a  change  of  frame,  i.e., 

[a]^[b]_4  =  [a]g[b]g  (2.138) 

Additionally, 

[axb]^  =  [Q]^[axb]g  (2.139) 

The  k\h  moment  of  a  x  w  matrix  T,  denoted  by  is  defined  as  (Leigh  1968) 


Ii=tr(T^).  A:  =  0,1,... 


(2.140) 


where  Xo  =  tr(l)  =  n.  Now  we  have 

Theorem  2.7.1.  The  moments  of  a  n  x  n  matrix  are  invariant  under  a  similarity 
transformation. 

Proof  This  is  straightforward.  Indeed,  from  Theorem  2.6.3,  we  have 

[T'^]s  =  [A-'U[T'^U[AU  (2.141) 

Now,  let  [Xk]A  and  [Tk]B  denote  the  k\h  moment  of  [  T  and  respectively. 
Thus, 

[Ikh  =  tr([A-M.4[T*^].4[AU)  ^  tr([AU[A-M.4[T*^].4) 

=  tr([T*^U)  =  [J,U 

thereby  completing  the  proof. 

Furthermore, 

Theorem  2.7.2.  An  xn  matrix  has  only  n  linearly  independent  moments. 

Proof.  Let  the  characteristic  polynomial  of  T  be 

P(A)  =  ao  aiX  +  •  •  •  +  an-iX^~^  +  A"  =  0 


(2.142) 
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Upon  application  of  the  Cayley-Hamilton  Theorem,  Eq.  (2.142)  leads  to 

flol  +  +  •  •  •  +  +  T"  =  0  (2.143) 

where  1  denotes  the  n  xn  identity  matrix. 

Now,  if  we  take  the  trace  of  both  sides  of  Eq.  (2.143),  and  Definition  (2.140)  is 
recalled,  one  has 


ciqXq  +  aiXi  +  •  •  •  +  cifi—iXf^—i  Xyi  —  0  (2.144) 

from  which  it  is  apparent  that  can  be  expressed  as  a  linear  combination  of  the  first 
n  moments  of  T,  {  2)^  }q“^  By  simple  induction,  one  can  likewise  prove  that  the  mth 
moment  is  dependent  upon  the  first  n  moments  if  m  >  n,  thereby  completing  the 
proof.  Also  notice  that  Xq  =  n,  and  hence,  all  n  matrices  share  the  same  zeroth 
moment  Xq. 

The  vector  invariants  of  a  ^  x  /7  matrix  are  its  eigenvectors,  which  have  a  direct 
geometric  significance  in  the  case  of  symmetric  matrices.  The  eigenvalues  of  these 
matrices  are  all  real,  its  eigenvectors  being  also  real  and  mutually  orthogonal. 
Skew-symmetric  matrices,  in  general,  need  not  have  either  real  eigenvalues  or  real 
eigenvectors.  However,  if  we  limit  ourselves  to  3  x  3  skew- symmetric  matrices, 
exactly  one  of  their  eigenvalues,  and  its  associated  eigenvector,  are  both  real. 
The  eigenvalue  of  interest  is  0,  and  the  associated  vector  is  the  axial  vector  of  the 
matrix  under  study. 

It  is  now  apparent  that  two  n  matrices  related  by  a  similarity  transformation 
have  the  same  set  of  moments.  Now,  by  virtue  of  Theorem  2.7.2,  one  may  be 
tempted  to  think  that  if  two  n  n  matrices  share  their  first  n  moments  {2^  }q“\ 
then  the  two  matrices  are  related  by  a  similarity  transformation.  To  prove  that  this 
is  not  the  case,  let  two  ny^n  matrices  A  and  B  have  characteristic  polynomials  with 
coefficients  {akY~^  and  respectively,  the  two  sets  being  not  necessarily 

identical.  Moreover,  let  the  n  -\-  \  moments  of  A  and  B  be  denoted  by  {2^}q  and 
with 


To  =  Iq  =  n,  Ik  =  I'j^,  for  k  =  1 , . . . , n  —  1 
Hence,  from  Eq.  (2.144), 

Xfi  =  —(aon  +  aiXi  +  •  •  •  +  an—iXn—i) 

2^  =  —(bon  +  biXi  +  •  •  •  +  bn-iX^-i) 

Therefore,  in  spite  of  Theorem  2.7.2,  two  n  x  n  matrices  with  identical  moments 
Xk  =  2^,  for  /:  =  I, ...  ,n  —  I  may  still  have  2„  ^  2'  if  these  matrices  are 

not  related  by  a  similarity  transformation,  and  hence,  have  distinct  characteristic 

polynomials.  We  thus  have 
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Theorem  2.7.3.  Two  n  n  matrices  are  related  by  a  similarity  transformation  if 
and  only  if  their  n  moments  identical. 

Hence, 

Corollary  2.7.1.  If  two  n^n  matrices  share  the  same  n  moments  {Xj^Yv  then  their 
characteristic  polynomials  are  identical. 

Consider  the  two  matrices  A  and  B  given  below: 


1  2 
2  1 


The  two  foregoing  matrices  cannot  possibly  be  related  by  a  similarity  transforma¬ 
tion,  for  the  first  one  is  the  identity  matrix,  while  the  second  is  not.  However,  the 
two  matrices  share  the  two  moments  Xq  =  2  and  Xi  =  2.  Let  us  now  compute  the 
second  moments  of  these  matrices: 


trCA^) 


tr(B^)  =  tr 


5  4 
4  5 


10 


which  are  indeed  different.  Therefore,  to  test  whether  two  different  n  y^n  matrices 
represent  the  same  linear  transformation,  and  hence,  are  related  by  a  similarity 
transformation,  we  must  verify  that  they  share  the  same  set  of  /7  + 1  moments  {  }q  . 
In  fact,  since  all  /2  x  matrices  share  the  same  zeroth  moment,  only  the  n  moments 
{  Xk  Y  need  be  tested  for  similarity  verification.  That  is,  if  two  n  xn  matrices  share 
the  same  n  moments  { X^  } then  they  represent  the  same  linear  transformation, 
albeit  in  different  coordinate  frames. 

The  foregoing  discussion  does  not  apply,  in  general,  to  nonsymmetric  matrices, 
for  these  matrices  are  not  fully  characterized  by  their  eigenvalues.  For  example, 
consider  the  matrix 


1  2 
0  1 


Its  two  moments  of  interest  are  Xi  =  tr(A)  =  2,  X2  =  2,  which  happen  to  be  the 
corresponding  moments  of  the  2x2  identity  matrix  as  well.  However,  while  the 
identity  matrix  leaves  all  two-dimensional  vectors  unchanged,  matrix  A  does  not. 

Now,  if  two  symmetric  matrices,  say  A  and  B,  represent  the  same  transformation, 
they  are  related  by  a  similarity  transformation,  i.e.,  a  nonsingular  matrix  T  exists 
such  that 


B  =  T“‘AT 

Given  A  and  T,  then,  finding  B  is  trivial,  a  similar  statement  holding  if  B  and 
T  are  given;  however,  if  A  and  B  are  given,  finding  T  is  more  difficult.  The  latter 
problem  occurs  sometimes  in  robotics  in  the  context  of  calibration,  to  be  discussed 
in  Sect.  2.7.1. 
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Example  2.7.1.  Two  symmetric  matrices  are  displayed  below.  Find  out  whether 
they  are  related  by  a  similarity  transformation. 


"1 0  r 

"1  0  0  ■ 

A  = 

0  1  0 

,  B  = 

0  2-1 

_1  0  2_ 

_0  -1  1  _ 

Solution:  The  traces  of  the  two  matrices  are  apparently  identical,  namely,  4.  Now 
we  have  to  verify  whether  their  second  and  third  moments  are  also  identical.  To  do 
this,  we  need  the  square  and  the  cube  of  the  two  matrices,  from  which  we  then 
compute  their  traces.  Thus,  from 


A2  = 

"2  0  3" 

0  1  0 

,  B^  = 

1  0 

0  5 

0  ■ 

-3 

_3  0  5_ 

1 

o 

1 

2  _ 

we  readily  obtain 

tr(A^)  =  trCB^)  =  8 

Moreover, 


A'  = 

"5  0  8  " 

0  1  0 

,  B^  = 

1  0  0 

0  13  -8 

_8  0  13_ 

1 

o 

1 

00 

1 _ 

whence 


tr(A^)  =  tr(B^)  =  19 

Therefore,  the  two  matrices  are  related  by  a  similarity  transformation.  Hence,  they 
represent  the  same  linear  transformation. 

Example  2. 7.2.  Same  as  Example  2.7.1,  for  the  two  matrices  displayed  below: 


"1  0  2' 

"1 1  r 

A  = 

0  1  0 

,  B  = 

1  1  0 

_2  0  0_ 

_1  0  o_ 

Solution:  As  in  the  previous  example,  the  traces  of  these  matrices  are  identical, 
i.e.,  2.  However,  tr(A^)  =  10,  while  tr(B^)  =  6.  We  thus  conclude  that  the  two 
matrices  cannot  be  related  by  a  similarity  transformation. 
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2.7.1  Applications  to  Redundant  Sensing 

A  sensor,  such  as  a  camera  or  a  range  finder,  is  often  mounted  on  a  robotic  end- 
effector  to  determine  the  pose — i.e.,  the  position  and  orientation,  as  defined  in 
Sect.  3.2.3 — of  an  object.  If  two  redundant  sensors  are  introduced,  and  we  attach 
frames  A  and  B  to  each  of  these,  then  each  sensor  can  be  used  to  determine  the 
orientation  of  the  end-effector  with  respect  to  a  reference  pose.  This  is  a  simple 
task,  for  all  that  is  needed  is  to  measure  the  rotation  R  that  each  of  the  foregoing 
frames  underwent  from  the  reference  pose,  in  which  these  frames  are  denoted  by 
and  Bo,  respectively.  Let  us  assume  that  these  measurements  produce  the  orthogonal 
matrices  A  and  B,  representing  R  in  ^  and  B,  respectively.  With  this  information 
we  would  like  to  determine  the  relative  orientation  Q  of  frame  B  with  respect  to 
frame  A,  a  problem  that  is  called  here  instrument  calibration. 

We  thus  have  A  =  and  B  =  and  hence,  the  algebraic  problem 

at  hand  consists  in  determining  [Q]^  or  equivalently,  [Q]/^-  The  former  can  be 
obtained  from  the  similarity  transformation  of  Eq.  (2.137),  which  leads  to 

A=  [Q]^B[Qn^ 


or 


A[QU  =  [Q]^B 

This  problem  could  be  solved  if  we  had  three  invariant  vectors  associated  with 
each  of  the  two  matrices  A  and  B.  Then,  each  corresponding  pair  of  vectors  of  these 
triads  would  be  related  by  Eq.  (2. 136),  thereby  obtaining  three  such  vector  equations 
that  should  be  sufficient  to  compute  the  nine  components  of  the  matrix  Q  rotating 
frame  A  into  B.  However,  since  A  and  B  are  orthogonal  matrices,  they  admit  only 
one  real  invariant  vector,  namely,  their  axial  vector,  and  we  are  short  of  two  vector 
equations.  We  thus  need  two  more  invariant  vectors,  represented  in  both  A  and  B,  to 
determine  Q.  The  obvious  way  of  obtaining  one  additional  vector  in  each  frame  is 
to  take  not  one,  but  two  measurements  of  the  orientation  of  and  Bo  with  respect 
to  A  and  B,  respectively.  Let  the  matrices  representing  these  orientations  be  given, 
in  each  of  the  two  coordinate  frames,  by  A/  and  B/,  for  i  =  1,2.  Moreover,  let 
and  b; ,  for  /  =  1 , 2,  be  the  axial  vectors  of  matrices  Af  and  B/ ,  respectively. 

Now,  if  none  of  ai  and  a2  vanishes,  and  the  two  vectors  are  linearly  independent, 
a  third  vector  can  be  obtained  out  of  each  pair,  namely, 

a3  =  ai  X  a2,  b3  =  bi  x  b2  (2.145) 

If  one  of  the  vectors  of  the  two  pairs  vanishes,  we  have  two  more  possibilities, 
namely,  the  angle  of  rotation  of  that  orthogonal  matrix,  Ai  or  A2,  whose  axial  vector 
vanishes  is  either  0  or  tt.  If  the  foregoing  angle  vanishes,  then  A  underwent  a 
pure  translation  from  ^0,  the  same  holding,  of  course,  for  B  and  Bo.  This  means 
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that  the  corresponding  measurement  becomes  useless  for  our  purposes,  and  a  new 
measurement  is  needed,  involving  a  rotation.  If,  on  the  other  hand,  the  same  angle 
is  TT,  then  the  associated  rotation  is  symmetric  and  the  unit  vector  e  parallel  to  its 
axis  can  be  determined  from  Eq.  (2.50)  in  both  A  and  B.  This  unit  vector,  then, 
would  play  the  role  of  the  vanishing  axial  vector,  and  we  would  thus  end  up,  in 
any  event,  with  two  pairs  of  nonzero  vectors,  {a/  }\  and  {b/  }\.  Moreover,  the  pairs 
can  be  linearly  dependent  while  none  of  its  two  vectors  vanishes  and  the  vectors 
are  distinct.  This  is  possible  if  the  two  rotations  take  place  about  the  same  axis  but 
through  distinct  angles.  In  this  case,  the  second  rotation  becomes  useless,  should  be 
rejected,  and  a  new  second  rotation  must  be  taken.  In  conclusion,  we  can  always 
find  two  triads  of  nonzero  vectors,  {  a/  and  {  b/  }^,  that  are  related  by 

a.  =  [Q]^b,,  for/  =  1,2,3  (2.146) 

The  problem  at  hand  now  reduces  to  computing  [  Q  from  Eq.  (2.146).  In  order 
to  perform  this  computation,  we  write  the  three  foregoing  equations  in  matrix  form, 
namely. 


E  =  [QUF 


(2.147) 


with  E  and  F  defined  as 


E  =  [ai  32  33] ,  F  =  [bi  b2  b3]  (2.148) 

Now,  by  virtue  of  the  form  in  which  the  two  vector  triads  were  defined,  none  of  the 
two  above  matrices  is  singular,  and  hence,  we  have 

[Q]^  =  EF-'  (2.149) 


Moreover,  note  that  the  inverse  of  F  can  be  expressed  in  terms  of  its  columns 
explicitly,  without  introducing  components,  if  the  concept  of  reciprocal  bases  is 
recalled  (Brand  1965).  Thus, 


1 

A 


"(b2  X  b3)^" 
(b3  X  bi)^  , 
_(bi  X  b2)^_ 


A  =  bi  X  b2  •  bs 


Therefore, 


[ai(b2  X  b3)^  +  a2(b3  x  bi)^  +  a3(bi  x  b2)^] 


(2.150) 


(2.151) 


thereby  completing  the  computation  of  [Q]^  directly  and  with  simple  vector 
operations. 
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Fig.  2.8  Measuring  the  orientation  of  a  camera-fixed  coordinate  frame  with  respect  to  a  frame 
fixed  to  a  robotic  end-effector 


Example  2.7.3  (Hand-Eye  Calibration).  Determine  the  relative  orientation  of  a 
frame  B  attached  to  a  camera  mounted  on  a  robot  end-effector,  with  respect  to  a 
frame  A  fixed  to  the  latter,  as  shown  in  Fig.  2.8.  It  is  assumed  that  two  measurements 
of  the  orientation  of  the  two  frames  with  respect  to  frames  Ao  and  So  in  the  reference 
configuration  of  the  end-effector  are  available.  These  measurements  produce  the 
orientation  matrices  A/  of  the  frame  fixed  to  the  camera  and  B/  of  the  frame  fixed  to 
the  end-effector,  for  /  =  1,2.  The  numerical  data  of  this  example  are  given  below: 


-0.92592593 

-0.37037037 

-0.07407407 

Ai  = 

0.28148148 

-0.80740741 

0.51851852 

_-0. 25185185 

0.45925926 

0.85185185_ 

"-0. 83134406 

0.02335236 

-0.55526725" 

A2  = 

-0.52153607 

0.31240270 

0.79398028 

0.19200830 

0.94969269 

-0.24753503_ 

"-0. 90268482 

0.10343126 

-0.41768659" 

Bi  = 

0.38511568 

0.62720266 

-0.67698060 

0.19195318 

-0.77195777 

-0.60599932 
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-0.73851280  -0.54317226  0.39945305 

-0.45524951  0.83872293  0.29881721 

-0.49733966  0.03882952  -0.86668653 


Solution:  Shiu  and  Ahmad  (1987)  formulated  this  problem  in  the  form  of  a 
matrix  linear  homogeneous  equation,  while  Chou  and  Kamel  (1988)  solved  the 
same  problem  using  quaternions  and  very  cumbersome  numerical  methods  that 
involve  singular- value  computations.  The  latter  require  an  iterative  procedure  within 
a  Newton-Raphson  method,  itself  iterative,  for  nonlinear-equation  solving.  Other 
attempts  to  solve  the  same  problem  have  been  reported  in  the  literature,  but  these 
also  resorted  to  extremely  complicated  numerical  procedures  for  nonlinear-equation 
solving  (Chou  and  Kamel  1991;  Horaud  and  Dornaika  1995).  The  latter  proposed 
a  more  concise  method  based  on  quaternions — isomorphic  to  the  Euler-Rodrigues 
parameters — that  nevertheless  is  still  computationally  expensive. 

More  recently,  Daniilidis  (1999)  proposed  an  algorithm  based  on  dual  quater¬ 
nions  to  simultaneously  estimate  the  relative  pose  of  the  two  frames  of  interest. 
In  this  book  we  do  not  study  either  quaternions — at  least,  not  by  this  name — or 
dual  algebra;  the  former  are,  in  fact,  isomorphic  to  the  Euler-Rodrigues  parameters 
of  a  rotation,  which  were  introduced  in  Sect.  2.3.6.  Dual  algebra,  in  turn,  is 
used  to  manipulate  scalars,  vectors  and  matrices  comprising  one  rotation  and  one 
translation,  or  their  statics  counterparts,  one  moment  and  one  force  (Angeles  1988). 
In  the  above  reference,  Daniilidis  resorts  to  the  singular- value  decomposition  to  find 
the  relative  pose  in  question,  but  this  decomposition  slows  down  the  computational 
procedure.  Angeles  et  al.  (2000),  in  turn,  proposed  an  alternative  approach  based 
on  the  invariance  concepts  introduced  in  this  section,  that  leads  to  an  algorithm 
involving  only  linear  equations.  This  algorithm,  moreover,  relies  on  recursive  least- 
square  computations,  thereby  doing  away  with  singular- value  computations  and 
allowing  for  real-time  performance.  This  reference  and  (Daniilidis  1999)  report 
experimental  results. 

Eirst,  the  vector  of  matrix  A/ ,  represented  by  a/ ,  and  the  vector  of  matrix  B/ , 
represented  by  hi ,  for  /  =  1 , 2,  are  computed  from  simple  differences  of  the  off- 
diagonal  entries  of  the  foregoing  matrices,  followed  by  a  division  by  2  of  all  the 
entries  thus  resulting,  which  yields 


"-0. 02962963" 

0.07784121" 

ai  = 

0.08888889 

,  32  = 

-0.37363778 

0.32592593_ 

_-0.27244422_ 

"-0. 04748859" 

"-0. 12999385" 

bi  = 

-0.30481989 

,  b2  = 

0.44869636 

0.14084221_ 

0.04396138_ 

In  the  calculations  below,  16  digits  were  used,  but  only  eight  are  displayed. 
Eurthermore,  with  the  foregoing  vectors,  we  compute  as  and  bs  from  cross  products. 


2.8  Exercises 


85 


thus  obtaining 


as 


0.09756097 

0.01730293 

0.00415020 


-0.07655343 

-0.01622096 

-0.06091842 


Furthermore,  A  is  obtained  as 


A  =  0.00983460 

while  the  individual  rank-one  matrices  inside  the  brackets  of  Eq.  (2.151)  are 
calculated  as 


ai(b2  xbs)^  = 


0.00078822 

-0.00236467 

-0.00867044 

-0.00162359 

0.00779175 

0.00568148 


0.00033435 

-0.00100306 

-0.00367788 

0.00106467 

-0.00510945 

-0.00372564 

-0.00158253 

-0.00028067 

-0.00006732 


-0.00107955 

0.00323866 

0.01187508 

0.00175680 

-0.00843102 

-0.00614762 

-0.00594326 

-0.00105407 

-0.00025282 


a2(b3  xbi)^  = 


asCbi  xb2)^  = 


-0.00746863 

-0.00132460 

-0.00031771 


whence  Q  in  the  A  frame  is  readily  obtained  as 


-0.84436553 

0.41714750 

-0.33622873 


-0.01865909 

-0.65007032 

-0.75964911 


-0.53545750 

-0.63514856 

0.55667078 


thereby  completing  the  desired  computation. 


2.8  Exercises 

N.B.:  Unless  either  a  numerical  result  is  required  or  you  are  instructed  to  do 
otherwise,  do  not  resort  to  components  in  the  exercises  below. 

2. 1  Prove  that  the  range  and  the  null  space  of  any  linear  transformation  L  of  vector 
space  lA  into  vector  space  V  are  vector  spaces  as  well,  the  former  of  V,  the 
latter  of  lA. 
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2.2  Let  L  map  hi  into  V  and  6im{U}  =  n,  dim{V}  =  m.  Moreover,  let  IZ  and  AT  be 
the  range  and  the  null  space  of  L,  their  dimensions  being  p  and  v,  respectively. 
Show  that  p  +  V  =  n. 

2.3  Given  two  arbitrary  nonzero  vectors  u  and  v  in  8^,  find  the  matrix  P 
representing  the  projection  of  8^  onto  the  subspace  spanned  by  u  and  v. 

2.4  Verify  that  P,  whose  matrix  representation  in  a  certain  coordinate  system  is 
given  below,  is  a  projection.  Then,  describe  it  geometrically,  i.e.,  identify  the 
plane  onto  which  the  projection  takes  place.  Moreover,  find  the  null  space 
ofP. 


1 

3 


2  1  -1 
1  2  1 
-1  1  2 


2.5  If  for  any  three-dimensional  vectors  a  and  v,  matrix  A  is  defined  as 


9(a  X  v) 
9v 


then  we  have 


j  _  3(v  X  a) 

av 

Show  that  A  is  skew- symmetric  without  introducing  components. 

2.6  Let  u  and  v  be  any  three-dimensional  vectors,  and  define  T  as 

T  =  1  +  uv^ 


The  (unit)  eigenvectors  of  T  are  denoted  by  wi,  W2,  and  W3.  Show  that,  say,  wi 
and  W2  are  any  unit  vectors  perpendicular  to  v  and  different  from  each  other, 
whereas  W3  =  u/||u||.  Also  show  that  the  corresponding  eigenvalues,  denoted 
by  Ai,  A2,  and  A3,  associated  with  wi,  W2,  and  W3,  respectively,  are  given  as 

Ai  =  A2  =  1,  A3  =  1  +  u  •  V 


2.7  Show  that  if  u  and  v  are  any  three-dimensional  vectors,  then 

det(l  +  uv^)  =  1  +  u  •  V 
Hint:  Use  the  results  of  the  Exercise  2.6. 

2.8  For  the  two  unit  vectors  e  and  f  in  three-dimensional  space,  define  the  two 
reflections 


Ri=l-2ee^,  R2  =  l-2ff^ 
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Now,  show  that  Q  =  R1R2  is  a  rigid-body  rotation,  and  find  its  axis  and  its 
angle  of  rotation  in  terms  of  unit  vectors  e  and  f.  Again,  no  components  are 
permitted  in  this  exercise. 

2.9  State  the  conditions  on  the  unit  vectors  e  and  f,  of  two  reflections  Ri  and 
R2,  respectively,  under  which  a  given  rotation  Q  can  be  factored  into  the 
reflections  Ri  and  R2  given  in  the  foregoing  exercise.  In  other  words,  not 
every  rotation  matrix  Q  can  be  factored  into  those  two  reflections,  for  fixed  e 
and  f,  but  special  cases  can.  Identify  these  cases. 

2.10  For  given  three-dimensional,  non-zero  a  and  b,  find  v  that  verifies 

V  +  a  X  V  =  b 

When  finding  an  expression  for  v  in  terms  of  a  and  b,  answer  the  questions 
below: 

(a)  Is  it  possible  to  find  v  for  arbitrary  a  and  b?  If  so,  find  an  expression  for  v. 

(b)  Can  V  be  orthogonal  to  a?  If  so,  under  which  conditions? 

(c)  Can  V  be  orthogonal  to  b?  If  so,  under  which  conditions? 

2.1 1  Prove  that  the  eigenvalues  of  the  cross-product  matrix  of  the  unit  vector  e  are 

0,  j ,  and  —j ,  where  j  =  Then  show  that  the  eigenvectors  associated 

with  the  complex  eigenvalues  are  both  complex  mutually  orthogonal  vectors, 
and  find  expressions  thereof  in  terms  of  e.  Note:  Given  two  n-dimensional 
vectors  u  and  \  defined  over  the  complex  field,  their  scalar  product  is  defined 
as  u*v,  where  u*  stands  for  the  transpose  conjugate  ofn. 

2.12  Prove  that  the  eigenvalues  of  a  proper  orthogonal  matrix  Q  are  1,  e^^ ,  and 
e~j^ ,  with  (p  denoting  the  angle  of  rotation.  Hint:  Use  the  result  of  the 
foregoing  exercise  and  the  Cayley-Hamilton  Theorem. 

2.13  Find  the  axis  and  the  angle  of  rotation  of  the  proper  orthogonal  matrix  Q  given 
below  in  a  certain  coordinate  frame  jF. 

-1-2  2  ■ 

-2  -1  -2 
2  -2-l_ 

2.14  Prove  that  the  vect(-)  and  the  tr(-)  operators  are  linear  homogeneous,  i.e., 
that  vect(o'A  +  ^B)  =  Q'vect(A)  +  ^vect(B),  with  a  similar  expression  for 
tr(Q'A  +  ^B). 

2.15  Cayley’s  Theorem,  which  is  not  to  be  confused  with  the  Theorem  of  Cayley- 
Hamilton,  states  that  every  3x3  proper  orthogonal  matrix  Q  can  be  uniquely 
factored  as 


Q  =  (1  -  S)(l  +  S)-‘ 

where  S  is  a  skew- symmetric  matrix.  Find  a  general  expression  for  S  in  terms 
of  Q,  and  state  the  condition  under  which  this  factoring  is  not  possible. 
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2.16  Find  matrix  S  of  Cayley’s  factoring  for  Q  as  given  in  Exercise  2.13. 

2.17  If  Q  represents  a  rotation  about  an  axis  parallel  to  the  unit  vector  e  through  an 
angle  0,  then  the  Rodrigues  vector  p  of  this  rotation  can  be  defined  as 


Note  that  if  r  and  tq  denote  the  Euler-Rodrigues  parameters  of  the  rotation 
under  study,  then  p  =  r/ro.  Show  that 

p  =  — vect(S) 


for  S  given  in  Exercise  2.15. 

2.18  The  vertices  of  a  cube,  labeled  A,  B,  . . .,  H,  are  located  so  that  A,  B,  C, 
and  D,  as  well  as  E,  F,  G,  and  H ,  are  in  clockwise  order  when  viewed  from 
outside.  Moreover,  AE,  BH,  CG,  and  DF  are  edges  of  the  cube,  which  is  to 
be  manipulated  so  that  a  mapping  of  vertices  takes  place  as  indicated  below: 

A^D,B^C,C^G,  D^F 
E^A,  F^E,G^H,H^B 

Find  the  angle  of  rotation  and  the  angles  that  the  axis  of  rotation  makes  with 
edges  AB,  AD,  and  AE. 

2.19  (Euler  angles)  A  rigid  body  can  attain  an  arbitrary  configuration  starting  from 
any  reference  configuration,  0,  by  means  of  the  composition  of  three  rotations 
about  coordinate  axes,  as  described  below:  Attach  axes  Aq,  To,  and  Zq  to 
the  body  in  the  reference  configuration  and  rotate  the  body  through  an  angle  <j) 
about  Zo,  thus  carrying  the  axes  into  Ai,  Ti,  and  Zi  (=Zo),  respectively.  Next, 
rotate  the  body  through  an  angle  0  about  axis  Ti,  thus  carrying  the  axes  into 
A2,  T2,  and  Z2,  respectively.  Finally,  rotate  the  body  through  an  angle  \l/  about 
Z2  so  that  the  axes  coincide  with  their  desired  final  orientation,  A3,  T3,  and 
Z3.  Angle  is  chosen  so  that  axis  Z3  lies  in  the  plane  of  Zq  and  Ai,  whereas 
angle  0  is  chosen  so  as  to  carry  axis  Zi  (=Zo)  into  Z3  (=Z2).  Show  that  the 
rotation  matrix  carrying  the  body  from  configuration  0  to  configuration  3  is: 


cOcCpClj/  —  scpsij/  —cOc(l)S\l/  —  scpcxj/  sOccj) 
cOs<j)c\lr  +  c<j)s\lr  —cOs^s\[f  +  c(pc\lf  sOs<j) 
—sOcxlf  sOsxlf  cO 


where  c(-)  and  s(')  stand  for  cos(-)  and  sin(-),  respectively.  Moreover,  show 
that  a,  the  angle  of  rotation  of  Q  given  above,  obeys  the  relation 
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Hint:  Let  R/  be  the  rotation  carrying  frame^  J^i-\  into  Ti,  for  i  =  1,2,3. 
Then,  the  total  rotation  carrying  Fq  into  can  be  found  to  be  R1R2R3, 
provided  that  all  three  rotation  matrices  are  given  in  the  same  frame.  However, 
each  Ki  admits  a  simple  representation,  in  canonical  form,  in  Ti-\.  Hence,  to 
represent  ^i,  for  i  =2,3,  in  3Fo,  a  similarity  transformation  ala  Eqs.  (2.127) 
and  (2.128)  is  needed. 

2.20  Given  an  arbitrary  rigid-body  rotation  about  an  axis  parallel  to  the  unit  vector  e 
through  an  angle  0,  it  is  possible  to  find  both  e  and  0  using  the  linear  invariants 
of  the  rotation  matrix,  as  long  as  the  vector  invariant  does  not  vanish.  The  latter 
happens  if  and  only  if  0  =  0  or  tt.  Now,  if  0  =  0,  the  associated  rotation 
matrix  is  the  identity,  and  e  is  any  three-dimensional  vector;  if  0  =  tt,  then 
we  have 


Q(n)  =  Q^  =  -1  +  2ee^ 


whence  we  can  solve  for  ee^  as 


ee^  =  ^  (Q;t  +  1) 


1 


Now,  it  is  apparent  that  the  three  eigenvalues  of  are  real  and  the  associated 
eigenvectors  are  mutually  orthogonal.  Find  these. 

2.21  Explain  why  all  the  off-diagonal  entries  of  a  symmetric  rotation  matrix  cannot 
be  negative. 

2.22  The  three  entries  above  the  diagonal  of  a  3  x  3  matrix  Q  that  is  supposed  to 
represent  a  rotation  are  given  below: 


Without  knowing  the  other  entries,  explain  why  the  above  entries  are  unac¬ 
ceptable. 

2.23  Let  pi,  P2,  and  p3  be  the  position  vectors  of  three  arbitrary  points  in  three- 
dimensional  space.  Now,  define  a  matrix  P  as 


P  =  [Pl  P2  P3] 


Show  that  P  is  not  frame-invariant.  Hint:  Show,  for  example,  that  it  is  always 
possible  to  find  a  coordinate  frame  in  which  tr(P)  vanishes.  This  task  will  be 
eased  if  you  represent  the  position  vectors  of  the  three  points  in  a  suitable 
coordinate  frame  in  which  a  few  of  their  coordinates  will  vanish. 

2.24  For  P  defined  as  in  Exercise  2.23,  let 

q  =  tr(P^)  —  tr^(P) 


is  obviously  frame  Xi ,  L ,  Z, . 
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Show  that  q  vanishes  if  the  three  given  points  and  the  origin  are  collinear,  for 
P  represented  in  any  coordinate  frame. 

2.25  For  P  defined,  again,  as  in  Exercise  2.23,  show  that  PP^  is  invariant  under 
frame-rotations  about  the  origin,  and  becomes  singular  if  and  only  if  either 
the  three  given  points  are  collinear  or  the  origin  lies  in  the  plane  of  the  three 
points.  Note  that  this  matrix  is  more  singularity-robust  than  P. 

2.26  The  diagonal  entries  of  a  rotation  matrix  are  known  to  be  —0.5,  0.25,  and 
—0.75.  Find  the  off-diagonal  entries. 

2.27  Asa  generalization  to  the  foregoing  exercise,  discuss  how  you  would  go  about 
finding  the  off-diagonal  entries  of  a  rotation  matrix  whose  diagonal  entries  are 
known  to  be  a,  b,  and  c.  Hint:  This  problem  can  be  formulated  as  finding 
the  intersection  of  the  coupler  curve  of  a  four-bar  spherical  linkage  (Chiang 
1988),  which  is  a  curve  on  a  sphere,  with  a  certain  parallel  of  the  same  sphere. 

2.28  Shown  in  Fig.  2.9a  is  a  cube  that  is  to  be  displaced  in  an  assembly  operation 
to  a  configuration  in  which  face  EFGH  lies  in  the  XY  plane,  as  indicated  in 
Fig.  2.9b.  Compute  the  unit  vector  e  parallel  to  the  axis  of  the  rotation  involved 
and  the  angle  of  rotation  0,  for  0  <  ^  <  tt. 

2.29  The  axes  X\,  Yi,  Z\  of  a  frame  T\  are  attached  to  the  base  of  a  robotic 
manipulator,  whereas  the  axes  X2,  T2,  Z2  of  a  second  frame  T2  are  attached 
to  its  end-effector,  as  shown  in  Fig.  2.10.  Moreover,  the  origin  F  of  T2  has 
the  7^1 -coordinates  (1,  —1, 1).  Furthermore,  the  orientation  of  the  end  effector 
with  respect  to  the  base  is  defined  by  a  rotation  Q,  whose  representation  in  T\ 
is  given  by 


2.8  Exercises 


91 


Fig.  2.10  Robotic  EE  approaching  a  stationary  object  ABC 


1  1  -  1  +  V3' 

1  +  \/3  1  1-^/3 

1  -  ^/3  1  +  1 


(a)  What  are  the  end-effector  coordinates  of  point  C  of  Fig.  2.10? 

(b)  The  end-effector  is  approaching  the  ABC  plane  shown  in  Fig.  2.10.  What 
is  the  equation  of  the  plane  in  end-effector  coordinates?  Verify  your  result 
by  substituting  the  answer  to  (a)  into  this  equation. 

2.30  Shown  in  Fig.  2.11  is  a  cube  of  unit  side,  which  is  composed  of  two  parts. 
Frames  (Xq,  Tq,  Zq)  and  (Xi ,  Fi ,  Zi)  are  attached  to  each  of  the  two  parts,  as 
illustrated  in  the  figure.  The  second  part  is  going  to  be  picked  up  by  a  robotic 
gripper  as  the  part  is  transported  on  a  belt  conveyor  and  passes  close  to  the 
stationary  first  part.  Moreover,  the  robot  is  to  assemble  the  cube  by  placing 
the  second  part  onto  the  first  one  in  such  a  way  that  vertices  A\,  Bi,  Ci 
are  coincident  with  vertices  A^,  B^,  Q.  Determine  the  axis  and  the  angle  of 
rotation  that  will  carry  the  second  part  onto  the  first  one  as  described  above. 

2.31  A  piece  of  code  meant  to  produce  the  entries  of  rotation  matrices  is  being 
tested.  In  one  run,  the  code  produced  a  matrix  with  diagonal  entries  —0.866, 
—0.866,  —0.866.  Explain  how  without  looking  at  the  other  entries,  you  can 
decide  that  the  code  has  a  bug. 

2.32  Shown  in  Fig.  2.12  is  a  rigid  cube  of  unit  side  in  three  configurations.  The 
second  and  the  third  configurations  are  to  be  regarded  as  images  of  the  first 
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Fig.  2.11  Roboticized  assembly  operation 


Fig.  2.12  Three  configurations  of  a  cube 


one.  One  of  the  last  two  configurations  is  a  reflection,  and  the  other  is  a  rotation 
of  the  first  one.  Identify  the  rotated  configuration  and  find  its  associated 
invariants. 

2.33  Two  frames,  Q  and  C,  are  attached  to  a  robotic  gripper  and  to  a  camera 
mounted  on  the  gripper,  respectively.  Moreover,  the  camera  is  rigidly  attached 
to  the  gripper,  and  hence,  the  orientation  of  C  with  respect  to  Q,  denoted  by  Q, 
remains  constant  under  gripper  motions.  The  orientation  of  the  gripper  with 
respect  to  a  frame  B  fixed  to  the  base  of  the  robot  was  measured  in  both  Q 
and  C.  Note  that  this  orientation  is  measured  in  Q  simply  by  reading  the  joint 
encoders,  which  report  values  of  the  joint  variables,  as  discussed  in  detail 
in  Chap.  4.  The  same  orientation  is  measured  in  C  from  estimations  of  the 
coordinates  of  a  set  of  points  fixed  to  S,  as  seen  by  the  camera. 

Two  measurements  of  the  above-mentioned  orientation,  denoted  Ri  and 
R2,  were  taken  in  Q  and  C,  with  the  numerical  values  reported  below: 

0.667  0.333  0.667" 

-0.667  0.667  0.333  , 

-0.333  -0.667  0.667 


[Ri]a  = 
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[Rlk 


0.500  0  -0.866 

0  1.000  0 
0.866  0  0.500 


0.707  0.577  0.408 

0  0.577  -0.816 

-0.707  0.577  0.408 


[^2  ]c 


10  0 
0  0.346  -0.938 
0  0.938  0.346 


(a)  Verify  that  the  foregoing  matrices  represent  rotations. 

(b)  Verify  that  the  first  two  matrices  represent,  in  fact,  the  same  rotation  Ri, 
albeit  in  different  coordinate  frames. 

(c)  Repeat  item  (b)  for  R2. 

(d)  Find  [Q]^.  Is  your  computed  Q  orthogonal?  If  not,  can  the  error  be 
attributed  to  data-incompatibility?  to  roundoff-error  amplification? 

2.34  The  orientation  of  the  end-effector  of  a  given  robot  is  to  be  inferred  from 
joint-encoder  readouts,  which  report  an  orientation  given  by  a  matrix  Q  in 
-coordinates,  namely, 

-12  2' 

2-12 
2  2-1 


(a)  Show  that  the  above  matrix  can  indeed  represent  the  orientation  of  a  rigid 
body. 

(b)  What  is  Q  in  end-effector  coordinates,  i.e.,  in  a  frame  Tj,  if  Z7  is  chosen 
parallel  to  the  axis  of  rotation  of  Q? 

2.35  The  rotation  Q  taking  a  coordinate  frame  B,  fixed  to  the  base  of  a  robot,  into  a 
coordinate  frame  Q,  fixed  to  its  gripper,  and  the  position  vector  g  of  the  origin 
of  Q  have  the  representations  in  B  given  below: 


[Qh  =  \ 

1  1  -  1  +  V3' 

1  - 

1  +  1  1  -  V3 

-  [g]s  = 

73 

3 

_1  -  V3  1  +  1 

_1  +  73  _ 

Moreover,  let  p  be  the  position  vector  of  any  point  V  of  the  three-dimensional 
space,  its  coordinates  in  B  being  (x,  y,  z).  The  robot  is  supported  by  a 
cylindrical  column  C  of  circular  cross  section,  bounded  by  planes  TJi  and 
772.  These  are  given  below: 

C:  =  4;  TJi:  z  =  0;  112'.  z  =  10 

Find  the  foregoing  equations  in  Q  coordinates. 
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Fig.  2.13  An  elliptical  path 
on  an  inclined  plane 


2.36  A  certain  point  of  the  gripper  of  a  robot  is  to  trace  an  elliptical  path  of  semiaxes 
a  and  b,  with  center  at  C,  the  centroid  of  triangle  PQR,  as  shown  in  Fig.  2.13. 
Moreover,  the  semiaxis  of  length  a  is  parallel  to  edge  P  Q ,  while  the  ellipse 
lies  in  the  plane  of  the  triangle,  and  all  three  vertices  are  located  a  unit  distance 
away  from  the  origin. 

(a)  For  b  =  2(2/3,  the  gripper  is  to  keep  a  fixed  orientation  with  respect  to  the 
unit  tangent,  normal,  and  binormal  vectors  of  the  ellipse,  denoted  by  , 
e„ ,  and  ,  respectively."^  Determine  the  matrix  representing  the  rotation 
undergone  by  the  gripper  from  an  orientation  in  which  vector  is  parallel 
to  the  coordinate  axis  X,  while  e„  is  parallel  to  Y  and  to  Z.  Express  this 
matrix  in  X,  F,  Z  coordinates,  if  the  equation  of  the  ellipse,  in  parametric 
form,  is  given  as 

x'  =  a  cos cp,  y'  =  b  z  =  ^ 

the  orientation  of  the  gripper  thus  becoming  a  function  of  (p. 

(b)  Find  the  value  of  cp  for  which  the  angle  of  rotation  of  the  gripper,  with 
respect  to  the  coordinate  axes  X,  F,  Z,  becomes  tt. 

2.37  With  reference  to  Exercise  2.28,  find  Euler  angles  0,  0,  and  0  that  will  rotate 
the  cube  of  Fig.  2.9a  into  the  attitude  displayed  in  Fig.  2.9b.  For  a  definition  of 
Euler  angles,  see  Exercise  2.19 

2.38  Find  a  sequence  of  Euler  angles  0,  0,  and  0,  as  defined  in  Exercise  2.19,  that 
will  carry  triangle  Ai,  Bi,  Ci  into  triangle  Aq,  Q,  of  Fig.  2.11. 


account  of  curve  geometry  is  given  in  Sect.  1 1.2. 


Chapter  3 

Fundamentals  of  Rigid-Body  Mechanics 


3.1  Introduction 

The  purpose  of  this  chapter  is  to  lay  down  the  foundations  of  the  kinetostatics 
and  dynamics  of  rigid  bodies,  as  needed  in  the  study  of  multibody  mechanical 
systems.  With  this  background,  we  study  the  kinetostatics  and  dynamics  of  robotic 
manipulators  of  the  serial  type  in  Chaps.  5  and  7,  respectively,  while  devoting 
Chap.  6  to  the  study  of  trajectory  planning.  The  latter  requires,  additionally,  the 
background  of  Chap.  4.  A  special  feature  of  the  current  chapter  is  the  study  of 
the  relations  between  the  angular  velocity  of  a  rigid  body  and  the  time-rates  of 
change  of  the  various  sets  of  rotation  invariants  introduced  in  Chap.  2.  Similar 
relations  between  the  angular  acceleration  and  the  second  time-derivatives  of  the 
rotation  invariants  are  also  recalled,  the  corresponding  derivations  being  outlined  in 
Appendix  A. 

Furthermore,  an  introduction  to  the  extremely  useful  analysis  tool  known  as 
screw  theory  (Roth  1984)  is  included.  In  this  context,  the  concepts  of  twist  and 
wrench  are  introduced,  which  prove  in  subsequent  chapters  to  be  extremely  useful 
in  deriving  the  kinematic  and  static,  i.e.,  the  kinetostatic,  relations  among  the  various 
bodies  of  multibody  mechanical  systems. 


3.2  General  Rigid-Body  Motion  and  Its  Associated  Screw 

In  this  section  we  analyze  the  general  motion  of  a  rigid  body.  Thus,  let  A  and  P  be 
two  points  of  the  same  rigid  body  B,  the  former  being  a  particular  reference  point, 
whereas  the  latter  is  an  arbitrary  point  of  B,  as  shown  in  Fig.  3.1.  Moreover,  the 
position  vector  of  point  A  in  the  original  configuration  is  a,  and  the  position  vector 
of  the  same  point  in  the  displaced  configuration,  denoted  by  A',  is  a'.  Similarly,  the 
position  vector  of  point  P  in  the  original  configuration  is  p,  while  in  the  displaced 
configuration  B',  this  point  is  F*',  its  position  vector  being  p'.  Furthermore,  p'  is  to 

J.  Angeles,  Fundamentals  of  Robotic  Mechanical  Systems:  Theory,  Methods,  95 

and  Algorithms,  Mechanical  Engineering  Series  124,  DOI  10.1007/978-3-319-01851-5_3, 

©  Springer  International  Publishing  Switzerland  2014 


96 


3  Fundamentals  of  Rigid-Body  Mechanics 


Fig.  3.1  General  rigid-body 
displacement 


be  determined,  while  a,  a',  and  p  are  given,  along  with  the  rotation  matrix  Q.  Vector 
p— a  can  be  considered  to  undergo  a  rotation  Q  about  point  A  throughout  the  motion 
taking  the  body  from  the  original  to  the  final  configuration.  Since  vector  p  —  a  is 
mapped  into  p'  —  a'  under  the  above  rotation,  one  can  write 

p'-a'  =  Q(p-a)  (3.1) 

and  hence 

p'  =  a'  +  Q(p  -  a)  (3.2) 

which  is  the  relationship  sought.  Moreover,  let  and  d p  denote  the  displacements 

of  A  and  F,  respectively,  i.e., 

d^  =  a'  —  a,  dp  =  p  —  p  (3.3) 

From  Eqs.  (3.2)  and  (3.3)  one  can  readily  obtain  an  expression  for  d/?,  namely, 

d/.  =  a'  -  p  +  Q(p  -  a) 

=  a'  -  a  -  p  +  Q(p  -  a)  +  a 
=  d^  +  (Q-l)(p-a)  (3.4) 

" - V - " 

dp/A 

where  dp /a  denotes  the  relative  displacement  of  P  with  respect  to  A.  Notice  that 
this  displacement  is  caused  only  by  the  rotation.  What  Eq.  (3.4)  states  is  that  the 
displacement  of  an  arbitrary  point  E*  of  a  rigid  body,  of  position  vector  p  in  an 
original  configuration,  is  determined  by  the  displacement  of  one  certain  point  A 
and  the  concomitant  rotation  Q,  as  depicted  in  Fig.  3.2.  In  this  figure,  the  final 
configuration  B'  is  attained  via  an  intermediate  configuration  B" ,  which  is  in 
turn  attained  from  S  by  a  pure  translation.^  Then,  B'  is  attained  from  B"  by  a 
pure  rotation  Q,  of  axis  parallel  to  vector  e  and  of  angle  0.  Apparently,  once  the 


1 


A  body  undergoes  a  pure  translation  when  all  its  points  move  under  the  same  displacement. 
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Fig.  3.2  General  rigid-body 
displacement  attained  by  a 
combination  of  translation 
and  rotation 


B 


H 


displacement  of  P  is  known,  its  position  vector  p'  can  be  readily  determined. 
An  interesting  result  in  connection  with  the  foregoing  discussion  is  summarized 
below: 

Theorem  3.2.1.  Let  a  rigid  body  B  move  under  a  general  displacement,  of  rotation 
Q  with  axis  parallel  to  the  unit  vector  e.  Then,  the  displacements  of  all  the  points  of 
B  have  the  same  component  along  e. 

Proof  Multiply  both  sides  of  Eq.  (3.4)  by  e^,  thereby  obtaining 

e^dp  =  +  e^(Q  -  l)(p  -  a) 

Now,  the  second  term  of  the  right-hand  side  of  the  above  equation  vanishes  because 
Qe  =  e,  and  hence,  Q^e  =  e;  the  said  equation  thus  leads  to 

e^dp  =  e^d^  =  Jo  (3.5) 

thereby  showing  that  the  displacements  of  all  points  of  the  body  have  the  same 
projection  Jq  onto  the  axis  of  rotation,  q.e.d. 

As  a  consequence  of  the  foregoing  result,  we  have  the  classical  Mozzi-Chasles 
Theorem  (Mozzi  1763;  Chasles  1830;  Ceccarelli  1995),  namely. 

Theorem  3.2.2  (Mozzi  1763;  Chasles  1830).  Given  a  rigid  body  moving  with  a 
general  displacement,  a  set  of  its  points,  located  on  a  line  C,  undergo  identical  dis¬ 
placements  of  minimum  magnitude.  Moreover,  line  C  and  the  minimum-magnitude 
displacement  are  parallel  to  the  axis  of  the  rotation  involved,  as  illustrated 
in  Fig.  3.3. 

Proof.  The  proof  is  straightforward  in  light  of  Theorem  3.2.1,  which  allows  us  to 
express  the  displacement  of  an  arbitrary  point  P  as  the  sum  of  two  orthogonal 
components,  namely,  one  parallel  to  the  axis  of  rotation,  independent  of  P  and 
denoted  by  d\\ — read  “d-par” — and  one  perpendicular  to  this  axis,  denoted  by  d_L — 
read  “d-perp” — i.e.. 


d^j  =  dll  +  d_L 


(3.6a) 
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Fig.  3.3  The  Mozzi-Chasles 
theorem 


C 

where 

d||  =  =  do,  d_L  =  (1  —  ee^)dp  (3.6b) 

and  clearly,  do  is  a  constant,  as  defined  in  Eq.  (3.5),  while  d\\  and  d_L  are  mutually 
orthogonal.  Indeed,  orthogonality  is  apparent  by  definition,  but  is  also  verified 
below: 


dll  •  d_L  =  r/oe^(l  —  ee^)d75  =  do(e^  —  ^^)dp  =  0 


Now,  by  virtue  of  the  orthogonality  of  the  two  components  of  d/? ,  it  is  apparent  that 


+  ||dxf  =  ^/o'+||di|f 


for  the  displacement  dp  of  any  point  of  the  body.  Hence,  in  order  to  minimize  ||dp  | 
we  have  to  make  ||d_L||,  and  hence,  d_L  itself,  equal  to  zero,  i.e.,  we  must  have  dp 
parallel  to  e: 


=  ae 


for  a  certain  scalar  a.  That  is,  the  displacements  of  minimum  magnitude  of  the 
body  under  study  are  parallel  to  the  axis  of  Q,  thereby  proving  the  first  part  of  the 
Mozzi-Chasles  Theorem.  The  second  part  is  also  readily  proven  by  noticing  that 
if  T**  is  a  point  of  minimum  displacement  magnitude,  of  position  vector  p*,  its 
component  perpendicular  to  the  axis  of  rotation  must  vanish,  and  hence, 

d^=  (1  —  ee^)dp* 

=  (1  -  ee^)d^  +  (1  -  ee^)(Q  -  l)(p*  -  a)  =  0 
Upon  expansion  of  the  above  expression  for  d^,  we  obtain 

(1  -  ee^)d^  +  (Q  -  l)(p*  -  a)  =  0 
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Now  it  is  apparent  that  if  we  define  a  line  C  passing  through  and  parallel  to 
e,  then  the  position  vector  p*  +  Ae  of  any  of  its  points  P  satisfies  the  foregoing 
equation.  As  a  consequence,  all  points  of  minimum  displacement  magnitude  lie  in  a 
line  parallel  to  the  axis  of  rotation  of  Q,  q.e.d. 

An  important  implication  of  the  foregoing  theorem  is  that  a  rigid  body  can  attain 
an  arbitrary  configuration  from  a  given  original  one,  following  a  screw-like  motion 
of  axis  C  and  pitch  p,  the  latter  being  defined  presently.  Thus,  it  seems  appropriate 
to  call  C  the  screw  axis  of  the  rigid-body  motion. 

Note  that  do,  as  defined  in  Eq.  (3.5),  is  an  invariant  of  the  motion  at  hand.  Thus, 
associated  with  a  rigid-body  motion,  one  can  then  define  a  screw  of  axis  C  and 
pitch  p.  Of  course,  the  pitch  is  defined  as 


do  dpC  In  do 

p  =  —  =  — ^  or  p  =  - 

0  0  0 


(3.7) 


which  has  units  of  m/rad  or,  correspondingly,  of  m/turn.  Moreover,  the  angle  (j)  of  the 
rotation  involved  can  be  regarded  as  one  more  feature  of  this  motion.  This  angle  is, 
in  fact,  the  amplitude  associated  with  the  said  motion.  We  will  come  across  screws 
in  discussing  velocities  and  forces  acting  on  rigid  bodies,  along  with  their  pitches 
and  amplitudes.  Thus,  it  is  convenient  to  introduce  this  concept  at  this  stage. 


3.2.1  The  Screw  of  a  Rigid-Body  Motion 

The  screw  axis  C  is  totally  specified  by  a  given  point  Po  of  C  that  can  be  defined,  for 
example,  as  that  lying  closest  to  the  origin,  and  a  unit  vector  e  defining  its  direction. 
Expressions  for  the  position  vector  po  of  Po  in  terms  of  a,  a'  and  Q,  are  derived 
below: 

Since  Po  was  defined  as  the  point  of  C  lying  closest  to  the  origin,  po  obviously 
is  perpendicular  to  e,  i.e.. 


e^Po  =  0  (3.8) 

Moreover,  the  displacement  do  of  Po  is  parallel  to  the  vector  of  Q,  and  hence,  is 
identical  to  d\\  defined  in  Eq.  (3.6b),  i.e.,  it  satisfies 

(Q  -  l)do  =  0 

where  do  can  be  expressed  using  the  general  expression  for  the  displacement, 
Eq.  (3.4),  namely. 


do  =  +  (Q-  l)(po  -  a) 


(3.9a) 
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Now,  since  do  is  identical  to  dy,  we  have,  from  Eq.  (3.6b), 

d/i  +  (Q  -  l)(Po  -  a)  =  d||  =  ee^do 
But  from  Theorem  3.2.1, 


e^do  =  e^d^ 


and  so 


dx  +  (Q  -  l)(po  -  a)  =  ee^d^ 
or,  after  rearranging  terms, 

(Q  -  l)Po  =  (Q  -  l)a  -  (1  -  ee^)d^  (3.9b) 

which  cannot  be  solved  for  po  because,  as  the  reader  is  invited  to  show,  its  matrix 
coefficient,  Q  —  1,  is  singular,  with  null  space  spanned  by  vector  e.  Nevertheless, 
an  expression  for  po  can  be  found  if  Eq.  (3.8)  is  adjoined  to  Eq.  (3.9b),  thereby 
obtaining 


(3.10) 


where  A  is  a  4  x  3  matrix  and  b  is  a  four-dimensional  vector,  both  given  by 


(Q  -  l)a  -  (1  -  ee^)d^ 
0 


(3.11) 


Equation  (3.10)  cannot  be  solved  for  po  directly,  because  A  is  not  a  square  matrix. 
In  fact,  that  equation  represents  an  overdetermined  system  of  four  equations  and 
three  unknowns.  Thus,  in  general,  that  system  does  not  admit  a  solution.  However, 
the  four  equations  are  compatible,  and  hence,  in  this  particular  case,  a  solution  of 
that  equation,  which  turns  out  to  be  unique,  can  be  determined.  In  fact,  if  both  sides 
of  Eq.  (3.10)  are  multiplied  from  the  left  by  A^,  we  have 

A^Apo  =  A^b  (3.12) 

Moreover,  if  the  product  A^A,  which  is  a  3  x  3  matrix,  is  invertible,  then  po  can  be 
computed  from  Eq.  (3.12).  In  fact,  the  said  product  is  not  only  invertible,  but  also 
admits  an  inverse  that  is  rather  simple  to  derive,  as  shown  below.  Now  the  rotation 
matrix  Q  is  recalled  in  terms  of  its  natural  invariants,  namely,  the  unit  vector  e 
parallel  to  its  axis  of  rotation  and  the  angle  of  rotation  0  about  this  axis,  as  given  in 
Eq.  (2.49),  reproduced  below  for  quick  reference: 


rr-r  rj-f 

Q  =  ee  +  cos  0(1  —  ee  )  +  sin  0E 
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where  1  represents  the  3x3  identity  matrix  and  E  the  cross-product  matrix  of  e, 
as  introduced  in  Eq.  (2.38).  Further,  Eq.  (2.49)  is  substituted  into  Eq.  (3.11),  which 
yields 


A^A  =  2(1  —  cos  0)1  —  (1  —  2  cos  0)ee^  (3.13) 

It  is  now  apparent  that  the  foregoing  product  is  a  linear  combination  of  1  and  ee^ . 
This  suggests  that  its  inverse  is  very  likely  a  linear  combination  of  these  two  matrices 
as  well.  If  this  is  in  fact  true,  then  one  can  write 


(A^A)  ^  =  al  +  Pee 


T 


(3.14) 


coefficients  a  and  P  being  determined  from  the  condition  that  the  product  of  A^A 
by  its  inverse  should  be  1,  which  leads  to 


1 


a  = 


2(1  —  COS0) 


1—2  cos  0 
2(1  —  COS0) 


(3.15) 


and  hence. 


(A^A)-'  = 


1 


1—2  cos  0  j. 

1  +  - — ee 


2(1  —  cos  0)  2(1  —  cos  0) 


(3.16) 


On  the  other  hand,  from  Eq.  (3.11), 


A^b  =  (Q  -  1)'  [(Q  -  l)a  -  d^] 


J 


(3.17) 


Upon  solving  Eq.  (3.12)  for  po  and  substituting  relations  (3.16)  and  (3.17)  into  the 
expression  thus  resulting,  one  finally  obtains 


Po 


(Q-lf(Qa-aO 


2(1  —  COS0) 


for  0  ^  0 


(3.18) 


Apparently,  po  can  be  computed  as  long  as  0  ^  0.  If  0  =  0,  then  we  are 
in  the  presence  of  a  pure  translation,  all  the  points  of  the  body  undergoing  the 
same  displacement.  Therefore,  any  point  of  the  body  is  of  minimum-magnitude 
displacement. 

We  have  thus  defined  a  line  C  of  the  rigid  body  under  study  that  is  completely 
defined  by  its  point  of  position  vector  po  and  a  unit  vector  e  determining  its 
direction.  Moreover,  we  have  already  defined  the  pitch  of  the  associated  motion, 
Eq.  (3.7).  The  line  thus  defined,  along  with  the  pitch,  determines  the  screw  of  the 
motion  under  study. 
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Fig.  3.4  A  line  C  passing 
through  two  points 


3.2.2  The  Plucker  Coordinates  of  a  Line 

Alternatively,  the  screw  axis,  and  any  line  for  that  matter,  can  be  defined  more 
conveniently  by  its  Plucker  coordinates.  In  motivating  this  concept,  we  recall  the 
equation  of  a  line  C  passing  through  two  points  Pi  and  P2  of  position  vectors  pi 
and  p2,  as  shown  in  Fig.  3.4. 

If  point  P  lies  in  £,  then,  it  must  be  collinear  with  Pi  and  P2,  a  property  that  is 
expressed  as 


(P2  -  Pi)  X  (p  -  pi)  =  0 


or  upon  expansion. 


(P2-P1)  xp  +  pi  X  (P2-P1)  =  0  (3.19) 

If  we  now  introduce  the  cross-product  matrices  Pi  and  P2  of  vectors  pi  and  p2  in 
the  above  equation,  we  have  an  alternative  expression  for  the  equation  of  the  line, 
namely. 


(P2  -  Pl)p  +  Pi  X  (P2  -  Pi)  =  0 

The  above  equation  can  be  regarded  as  a  linear  equation  in  the  homogeneous 
coordinates  of  point  P ,  namely. 


[P2  -  Pi  Pi  X  (P2 


(3.20) 


It  is  now  apparent  that  the  line  is  defined  completely  by  two  vectors,  the 
difference  p2  —  Pi,  or  its  cross-product  matrix  for  that  matter,  and  the  cross  product 
Pi  X  (P2  —  Pi)-  We  will  thus  define  a  six-dimensional  array  containing  these  two 
vectors,  namely. 


yc  = 


P2  -Pl 
Pi  X  (P2  -Pl) 


(3.21) 
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whose  six  scalar  entries  are  the  Pliicker  coordinates  of  C.  Moreover,  if  we  let 


then  we  can  write 


P2  -Pl 
P2  -Pl 


n  =  Pl  X  e 


(3.22) 


Vc  = 


P2  -  Pl 


e 

n 


The  six  scalar  entries  of  the  above  array  are  the  normalized  Pliicker  coordinates 
of  C.  Vector  e  determines  the  direction  of  £,  while  n  determines  its  location;  n  can 
be  interpreted  as  the  moment  of  a  unit  force  parallel  to  e  and  of  line  of  action  C. 
Hence,  n  is  called  the  moment  of  C.  Henceforth,  only  the  normalized  Pliicker 
coordinates  of  lines  will  be  used.  For  brevity,  we  will  refer  to  these  simply  as  the 
Pliicker  coordinates  of  the  line  under  study.  The  Pliicker  coordinates  thus  defined 
will  be  stored  in  a  Pliicker  array  k  in  the  form 


K  = 


e 

n 


(3.23) 


where  for  conciseness,  we  have  dropped  the  subscript  £,  while  assuming  that  the 
line  under  discussion  is  self-evident. 

Note,  however,  that  the  six  components  of  the  Pliicker  array,  i.e.,  the  Pliicker 
coordinates  of  line  £,  are  not  independent,  for  they  obey 


e-e=l,  n-e  =  0 


(3.24) 


and  hence,  any  line  C  has  only  four  independent  Pliicker  coordinates.  In  the 
foregoing  paragraphs,  we  have  talked  about  the  Pliicker  array  of  a  line,  and  not  about 
the  Pliicker  vector;  the  reason  for  this  distinction  is  given  below.  The  set  of  Pliicker 
arrays  is  a  clear  example  of  an  array  of  real  numbers  not  constituting  a  vector 
space.  What  disables  Pliicker  arrays  from  being  vectors  are  the  two  constraints 
that  their  components  must  satisfy,  namely,  (a)  the  sum  of  the  squares  of  the  first 
three  components  of  a  Pliicker  array  is  unity,  and  (b)  the  unit  vector  of  a  line  is 
normal  to  the  moment  of  the  line.  Nevertheless,  we  can  perform  with  Pliicker  arrays 
certain  operations  that  pertain  to  vectors,  as  long  as  we  keep  in  mind  the  essential 
differences.  For  example,  we  can  multiply  Pliicker  arrays  by  matrices  of  the  suitable 
dimension,  with  entries  having  appropriate  units,  as  we  will  show  presently. 

It  must  be  pointed  out  that  a  Pliicker  array  is  dependent  upon  the  location  of 
the  point  with  respect  to  which  the  moment  of  the  line  is  measured.  Indeed,  let  k  a 
and  K  B  denote  the  Pliicker  arrays  of  the  same  line  C  when  its  moment  is  measured 
at  points  A  and  B,  respectively.  Moreover,  this  line  passes  through  a  point  P  of 
position  vector  p  for  a  particular  origin  O .  Now,  let  the  moment  of  C  with  respect 
to  A  and  B  be  denoted  by  and  n^,  respectively,  i.e.. 


=  (p  -  a)  X  e,  =  (p  -  b)  x  e 


(3.25) 
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and  hence, 


Obviously, 


i.e.. 


e 

e 

Ka  = 

,  icb  = 

115  —  =  (a  —  b)  X  e 


icb 


e 

+  (a  —  b)  X  e 


which  can  be  rewritten  as 


Kb  =  Ua:^ 


with  the  6x6  matrix  U  defined  as 


1  O 
A-B  1 


(3.26) 


(3.27) 


(3.28) 


(3.29a) 


(3.29b) 


while  A  and  B  are,  respectively,  the  cross-product  matrices  of  vectors  a  and  b,  and 
O  denotes  the  3x3  zero  matrix.  Given  the  lower-triangular  structure  of  matrix  U,  its 
determinant  is  simply  the  product  of  its  diagonal  entries,  which  are  all  unity.  Hence, 

det(U)  =  1  (3.30) 


U  thus  belonging  to  the  unimodular  group  of  6  x  6  matrices.  These  matrices  are 
rather  simple  to  invert.  In  fact,  as  one  can  readily  prove. 


1  O 
B-A  1 


(3.31) 


Relations  (3.29a  and  b)  can  then  be  called  the  Plucker-coordinate  transfer  formulas. 

Note  that  upon  multiplication  of  both  sides  of  Eq.  (3.27)  by  (a  —  b), 

(a  —  b)^n5  =  (a  —  b)^n^  (3.32) 

and  hence,  the  moments  of  the  same  line  C  with  respect  to  two  points  are  not 
independent,  for  they  have  the  same  component  along  the  line  joining  the  two  points. 

A  special  case  of  a  line,  of  interest  in  kinematics,  is  a  line  at  infinity.  This 
is  a  line  with  undefined  orientation,  but  with  a  defined  direction  of  its  moment; 
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this  moment  is,  moreover,  independent  of  the  point  with  respect  to  which  it  is 
measured.  Very  informally,  the  Pliicker  coordinates  of  a  line  at  infinity  can  be 
derived  from  the  general  expression,  Eq.  (3.23),  if  we  rewrite  it  in  the  form 


where  clearly  n/||n||  is  a  unit  vector;  henceforth,  this  vector  will  be  denoted  by  f. 
Now  let  us  take  the  limit  of  the  above  expression  as  P  goes  to  infinity,  i.e.,  when 
^  oo,  and  consequently,  as  ||n||  ^  oo.  Thus, 


lim  K  = 

nll^cx) 


fe/l 

n| 

f 

whence 


lim  K 

nll^oo 


0 

f 


The  six-dimensional  array  appearing  in  the  above  equation  is  defined  as  the  Pliicker 
array  of  a  line  at  infinity,  namely. 


^OO  — 


0 

f 


(3.33) 


Note  that  a  line  at  infinity  of  unit  moment  f  can  be  thought  of  as  being  a  line  lying  in 
a  plane  perpendicular  to  the  unit  vector  f,  but  otherwise  with  an  indefinite  location 
in  the  plane,  except  that  it  is  an  infinitely  large  distance  from  the  origin.  Thus,  lines 
at  infinity  vary  only  in  the  orientation  of  the  plane  in  which  they  lie. 


3.2.3  The  Pose  of  a  Rigid  Body 

A  possible  form  of  describing  a  general  rigid-body  motion,  then,  is  through  a  set 
of  eight  real  numbers,  namely,  the  six  Pliicker  coordinates  of  its  screw  axis,  its 
pitch,  and  its  amplitude,  i.e.,  its  angle.  Hence,  a  rigid-body  motion  is  fully  described 
by  six  independent  parameters.  Moreover,  the  pitch  can  attain  values  from  — oo  to 
+00.  Alternatively,  a  rigid-body  displacement  can  be  described  by  seven  dependent 
parameters  as  follows:  four  invariants  of  the  concomitant  rotation — the  linear 
invariants,  the  natural  invariants,  or  the  Euler-Rodrigues  parameters,  introduced  in 
Sect.  2.3 — and  the  three  components  of  the  displacement  of  an  arbitrary  point.  Since 
those  invariants  are  not  independent,  but  subject  to  one  constraint,  this  description 
consistently  involves  six  independent  parameters.  Thus,  let  a  rigid  body  undergo  a 
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general  motion,  of  rotation  Q  from  a  reference  configuration  Cq.  If  a  landmark  point 
A  of  the  body  undergoes  a  displacement  d^,  then  the  pose  array,  or  simply  the  pose 
s,  of  the  body  in  configuration  C  is  defined  as  a  seven-dimensional  array,  namely. 


q 


s  = 


(3.34) 


where  the  three-dimensional  vector  q  and  the  scalar  qo  are  any  four  invariants  of  Q. 
For  example,  if  these  are  the  Euler-Rodrigues  parameters,  then 

<P'\  (4> 

-je,  ^o-cos(^- 

If  alternatively,  we  work  with  the  linear  invariants,  then 

q  =  (sin  0)e,  =  cos  0 

and,  of  course,  if  we  work  instead  with  the  natural  invariants,  then 


q  =  e,  ^0  =  0 


In  the  first  two  cases,  the  constraint  mentioned  above  is 


In  the  last  case,  the  constraint  is  simply 


(3.35) 


(3.36) 


Notice  that  the  pose  of  a  rigid  body  is  an  array  of  numbers  quantifying  the  displace¬ 
ment  of  the  body  from  a  reference  configuration  Co  to  a  current  configuration  C.  As 
such,  then,  the  pose  is  a  relative  concept. 

An  important  problem  in  kinematics  is  the  computation  of  the  screw  parameters, 
i.e.,  the  components  of  s,  as  given  in  Eq.  (3.34),  from  coordinate  measurements  over 
a  certain  finite  set  of  points.  From  the  foregoing  discussion,  it  should  be  clear  that 
the  computation  of  the  attitude  of  a  rigid  body,  given  by  matrix  Q  or  its  invariants, 
is  crucial  in  solving  this  problem.  Moreover,  besides  its  theoretical  importance, 
this  problem,  known  as  pose  estimation,  has  also  practical  relevance.  Actually, 
the  problem  of  interest  can  be  regarded  as  one  of  relative-pose  estimation,  as  the 
pose  is,  in  reality,  a  relative  concept.  In  space  applications,  for  example,  a  common 
operation  is  the  rendezvousing  of  two  objects,  e.g.,  the  now-defunct  Space  Shuttle 
with  the  International  Space  Station.  Another  example  is  the  grabbing  of  objects 
floating  in  space,  e.g.,  a  drifting  satellite,  by  a  robotic  gripper.  In  order  to  properly 
orient  the  gripper,  the  relative  pose  of  the  satellite  with  respect  to  the  gripper  palm 
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Fig.  3.5  MDA  Space  Vision 
Marker  System  (SVMS)  at 
MDAs  Space  Vision 
Laboratory  in  Brampton, 
Ontario,  Canada.  Developed 
under  contract  with  the 
Canadian  Space  Agency 


B 


Fig.  3.6  Decomposition  of  the  displacement  of  a  rigid  body 


is  needed.  This  can  be  estimated  by  means  of  markers  previously  attached  on  the 
satellite  surface,  that  identify  corresponding  points  thereof.  These  markers  are  not 
simple  points,  but  arrays  of  visual  features  similar  to  the  simple  one-dimensional  and 
two-dimensional  barcodes,  although  more  evolved,  to  package  a  larger  amount  of 
information  in  the  same  area.  The  purpose  here  is  to  add  robustness  to  the  estimation 
algorithm  in  use.  A  new  concept,  the  Space  Vision  Marker  System  (SVMS),  was 
introduced  by  a  MDA  Team  (Bondy  et  al.  2007),  that  uses  markers  of  the  kind 
shown  in  Fig.  3.5,  along  with  estimation  algorithms.  These  markers  are  aimed  at 
marker-detection  throughout  “a  wide  range  of  viewing  distances  and  angles,  and 
illumination  (including  direct  sun  light  and  shadows);  encoded  redundant  features 
allow  identification  even  with  a  partial  data  loss.” 

A  straightforward  method  of  computing  the  screw  parameters  consists  in  regard¬ 
ing  the  motion  as  follows:  Choose  a  certain  point  A  of  the  body,  of  position  vector  a, 
and  track  it  as  the  body  moves  to  a  displaced  configuration,  at  which  point  A 
moves  to  A',  of  position  vector  a'.  Assume  that  the  body  reaches  the  displaced 
configuration  B',  passing  through  an  intermediate  one  B",  which  is  attained  by  a 
pure  translation,  as  depicted  in  Fig.  3.2.  Next,  configuration  B'  is  reached  by  rotating 
the  body  about  point  A',  as  indicated  in  Fig.  3.6. 
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Matrix  Q  can  now  be  readily  determined.  To  do  this,  define  three  points  of 

the  body,  Pi,  P2,  and  P3,  in  such  a  way  that  the  three  vectors  defined  below  are 

orthonormal  and  form  a  right-hand  system: 

ei  =  A^i,  02  =  ZK,  03  =  (3.37) 

0,-  •  ej  =  8ij ,  i,  j  =  1,2,3,  03  =  01  X  02  (3.38) 

where  8ij  is  the  Kronecker  delta,  defined  as  1  if  /  =  j ,  and  as  0  otherwise.  Now, 
let  the  set  {0/}i  be  labeled  {ejji  and  {ej'li  in  configurations  B'  and  B" ,  respectively. 
Moreover,  let  qtj  denote  the  entries  of  the  matrix  representation  of  the  rotation  Q  in 
a  frame  X,  Y,  Z  with  origin  at  A  and  such  that  the  foregoing  axes  are  parallel  to 
vectors  01,  02,  and  03,  respectively.  It  is  clear,  from  Definition  2.2.1,  that 


i.e.. 


(3.39) 


01  •  01  01  •  02  ei  •  03 

02  •  01  02  •  e2  02  •  e3 

03  •  01  03  •  02  e3  •  03 


(3.40) 


Note  that  all  0,  and  0J  appearing  in  Eq.  (3.40)  must  be  represented  in  the  same 
coordinate  frame.  Once  Q  is  determined,  computing  the  remaining  screw  parameters 
is  straightforward.  One  can  use,  for  example,  Eq.  (3.18)  to  determine  the  point  of  the 
screw  axis  that  lies  closest  to  the  origin,  which  would  thus  allow  one  to  compute  the 
Pliicker  coordinates  of  the  screw  axis. 


3.3  Rotation  of  a  Rigid  Body  About  a  Fixed  Point 

In  this  section,  the  motion  of  a  rigid  body  having  a  point  fixed  is  analyzed.  This 
motion  is  fully  described  by  a  rotation  matrix  Q  that  is  proper  orthogonal.  Now,  Q 
will  be  assumed  to  be  a  smooth  function  of  time,  and  hence,  the  position  vector  of  a 
point  P  in  an  original  configuration,  denoted  here  by  po,  is  mapped  smoothly  into  a 
new  vector  p(^),  namely. 


p(0  =  Q(OPo  (3.41) 

The  velocity  of  P  is  computed  by  differentiating  both  sides  of  Eq.  (3.41)  with 
respect  to  time,  thus  obtaining 


p(f)  =  Q(Opo 


(3.42) 
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which  is  not  a  very  useful  expression,  because  it  requires  knowledge  of  the  original 
position  of  P .  An  alternative  expression  can  be  derived  if  Eq.  (3.41)  is  solved  for  po 
and  the  expression  thus  resulting  is  substituted  into  Eq.  (3.42),  which  yields 

P  =  QQ^P  (3.43) 

where  the  argument  t  has  been  dropped  for  the  sake  of  simplicity,  but  one  must  keep 
in  mind  that  all  quantities  are  now  time- varying.  The  product  QQ^  is  known  as  the 
angular-velocity  matrix  of  the  rigid-body  motion  and  is  denoted  by  ,  i.e., 

ft  =  QQ^  (3.44) 

As  a  consequence  of  the  orthogonality  of  Q,  one  has  a  basic  result,  namely. 

Theorem  3.3.1.  The  angular-velocity  matrix  is  skew -symmetric. 

Proof.  This  follows  directly  from  definition  (3.44). 

In  order  to  derive  the  angular-velocity  vector  of  a  rigid-body  motion,  we  recall 
the  concept  of  axial  vector,  or  simply  vector,  of  a  3  x  3  matrix,  introduced  in 
Sect.  2.3.3.  Thus,  the  angular- velocity  vector  oo  of  the  rigid-body  motion  under  study 
is  defined  as  the  vector  of  i.e.. 


(o  =  vect(S^)  (3.45) 

and  hence,  Eq.  (3.43)  can  be  written  as 

p  =  =  (3.46) 

from  which  it  is  apparent  that  the  velocity  of  any  point  P  of  a  body  moving  with  a 
point  O  fixed  is  perpendicular  to  line  OP . 


3.4  General  Instantaneous  Motion  of  a  Rigid  Body 

If  a  rigid  body  now  undergoes  the  most  general  motion,  none  of  its  points  remains 
fixed,  and  the  position  vector  of  any  of  these,  T*,  in  a  displaced  configuration  is 
given  by  Eq.  (3.2).  Let  ao  and  po  denote  the  position  vectors  of  points  A  and  P 
of  Sect.  3.2,  respectively,  in  the  reference  configuration  Co,  a(0  and  p(t)  being  the 
position  vectors  of  the  same  points  in  the  displaced  configuration  C.  Moreover,  if 
Q(t)  denotes  the  rotation  matrix,  then 

p(0  =  a(0  +  Q(f)(Po  -  ao)  (3.47) 

Now,  the  velocity  of  P  is  computed  by  differentiating  both  sides  of  Eq.  (3.47)  with 
respect  to  time,  thus  obtaining 


p(f)  =  a(f)  +  Q(0(Po  -  ao) 


(3.48) 


no 
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which  again,  as  expression  (3.48),  is  not  very  useful,  for  it  requires  the  values  of  the 
position  vectors  of  A  and  F  in  the  original  configuration.  However,  if  Eq.  (3.47)  is 
solved  for  po  —  ao  and  the  expression  thus  resulting  is  substituted  into  Eq.  (3.48),  we 
obtain 


p  =  a  +  (p  —  a)  (3.49) 

or  in  terms  of  the  angular- velocity  vector, 

p  =  a  +  a>x(p  —  a)  (3.50) 

where  the  argument  t  has  been  dropped  for  brevity  but  is  implicit,  since  all  variables 
of  the  foregoing  equation  are  now  functions  of  time.  Eurthermore,  from  Eq.  (3.50), 
it  is  apparent  that 


(p  -  a)  •  (p  -  a)  =  0 


(3.51) 


which  can  be  summarized  as 

Theorem  3.4.1.  The  relative  velocity  of  two  points  of  the  same  rigid  body  is 
perpendicular  to  the  line  joining  them. 

Moreover,  similar  to  the  outcome  of  Theorem  3.2.1,  one  now  has  an  additional 
result  that  is  derived  upon  dot-multiplying  both  sides  of  Eq.  (3.50)  by  cu,  namely, 

0)  •  p  =  0)  •  a 


and  hence. 

Corollary  3.4.1.  The  projections  of  the  velocities  of  all  the  points  of  a  rigid  body 
onto  the  angular-velocity  vector  are  identical. 

Similar  to  the  Mozzi-Chasles  theorem,  we  have 

Theorem  3.4.2.  Given  a  rigid  body  under  general  motion,  a  set  of  its  points  located 
on  a  line  C'  undergoes  the  identical  minimum-magnitude  velocity  Vq  parallel  to  the 
angular  velocity. 

Definition  3.4.1.  The  line  containing  the  points  of  a  rigid  body  undergoing 
minimum-magnitude  velocities  is  called  the  instant  screw  axis  (ISA)  of  the  body 
under  the  given  motion. 


3.4.1  The  Instant  Screw  of  a  Rigid-Body  Motion 

Erom  Theorem  3.4.2,  the  instantaneous  motion  of  a  body  is  equivalent  to  that  of  the 
nut  of  a  screw  of  axis  C' ,  the  ISA.  As  the  body  moves,  the  ISA  changes,  for  which 
reason  the  motion  of  the  body  is  called  an  instantaneous  screw.  Moreover,  since  Vq 
is  parallel  to  o) ,  it  can  be  expressed  in  the  form 
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(0 


Vo  =  Vo 


(3.52) 


where  Vq  is  a  scalar  quantity  denoting  the  signed  magnitude  of  Vq  and  bears  the  sign 
of  Vo  •  cu .  Furthermore,  the  pitch  p'  of  the  instantaneous  screw  is  defined  as 


Vo 


p  •  (O 


In  Vo 


— 

or  p  — 

(0  1 

1 

(0  1 

^  ||a)|| 

(3.53) 


which  thus  bears  units  of  m/rad  or,  correspondingly,  of  m/turn. 

Again,  the  ISA  C'  can  be  specified  uniquely  through  its  Pliicker  coordinates, 
stored  in  the  pc'  array  defined  as 


Vc'  = 


(3.54) 


where  e'  and  n'  are,  respectively,  the  unit  vector  defining  the  direction  of  C'  and  its 
moment  about  the  origin,  i.e.. 


=  p  X  e' 


(3.55) 


p  being  the  position  vector  of  any  point  of  the  ISA.  Clearly,  e'  is  defined  uniquely  but 
becomes  trivial  when  the  rigid  body  instantaneously  undergoes  a  pure  translation, 
i.e.,  a  motion  during  which,  instantaneously,  o)  =  0  and  all  body-points  move  with 
the  same  velocity  vq.  In  this  case,  e'  is  defined  as  the  unit  vector  parallel  to  vq. 
Thus,  an  instantaneous  rigid-body  motion  is  defined  by  a  line  a  pitch  p' ,  and 
an  amplitude  ||a)||.  Such  a  motion  is,  then,  fully  determined  by  six  independent 
parameters,  namely,  the  four  independent  Pliicker  coordinates  of  its  pitch,  and 
its  amplitude.  A  line  supplied  with  a  pitch  is,  in  general,  called  a  screw;  a  screw 
supplied  with  an  amplitude  representing  the  magnitude  of  an  angular  velocity 
provides  the  representation  of  an  instantaneous  rigid-body  motion  that  is  sometimes 
called  the  twist,  an  item  that  will  be  discussed  in  more  detail  below. 

Hence,  the  instantaneous  screw  is  fully  defined  by  six  independent  real  numbers. 
Moreover,  such  as  in  the  case  of  the  screw  motion,  the  pitch  of  the  instantaneous 
screw  can  attain  values  from  —  oo  to  +oo. 

The  ISA  can  be  alternatively  described  in  terms  of  the  position  vector  Pq  of  its 
point  lying  closest  to  the  origin.  Expressions  for  Pq  in  terms  of  the  position  and 
the  velocity  of  an  arbitrary  body-point  and  the  angular  velocity  are  derived  below. 
To  this  end,  we  decompose  p  into  two  orthogonal  components,  py  and  p_L,  along  and 
transverse  to  the  angular- velocity  vector,  respectively — see  Eqs.  (2.6a)  and  (2.6b). 
Hence,  a  is  first  decomposed  into  two  such  orthogonal  components,  ay  and  the 
former  being  parallel,  the  latter  normal  to  the  ISA,  i.e.. 


a  =  ay  +  a_L 


(3.56) 
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These  orthogonal  components  are  given  as 


(O 


an  =  a  •  0) 


(O 


(0(0^  .  .  f  0)0)^ 

a,  a_L  =  1  - 


(O 


(O 


a  =  — 


(O 


(3.57) 


In  the  derivation  of  Eq.  (3.57)  we  have  used  the  identity  introduced  in  Eq.  (2.40), 
which  means,  in  our  case: 


=  (0(0^ 


(O 


(3.58) 


Upon  substitution  of  Eqs.  (3.57)  into  Eq.  (3.50),  we  obtain 


0)0)^  .  1 


S^^a  +  S^(p 

■  “vr  ■ 


p 


p± 


a) 


(3.59) 


Of  the  three  components  of  p,  the  first,  henceforth  referred  to  as  its  axial  component, 
is  parallel,  the  last  two  being  normal  to  oo.  The  sum  of  the  last  two  components  is 
referred  to  as  the  normal  component  of  p.  Erom  Eq.  (3.59)  it  is  apparent  that  the  axial 
component  is  independent  of  p,  while  the  normal  component  is  a  linear  function  of 
p.  An  obvious  question  now  arises:  For  an  arbitrary  motion,  is  it  possible  to  find 
a  certain  point  of  position  vector  p  whose  velocity  normal  component  vanishes  ? 
The  vanishing  of  the  normal  component  obviously  implies  the  minimization  of  the 
magnitude  of  p.  The  condition  under  which  this  happens  can  now  be  stated  as 


or 


(p  —  a) 


(O 


(3.60) 


which  can  be  further  expressed  as  a  vector  equation  linear  in  p,  namely. 


(3.61) 


or 


(p  —  r)  =  0 


(3.62a) 


with  r  defined  as 


1 


r  =  a  + 


(3.62b) 
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and  hence,  a  possible  solution  of  the  foregoing  problem  is 


(o  W 


(3.63) 


However,  this  solution  is  not  unique,  for  Eq.  (3.62a)  does  not  require  that  p  —  r  be 
zero,  only  that  this  difference  lie  in  the  null  space  of  i.e.,  that  p  —  r  be  linearly 
dependent  with  oo .  In  other  words,  if  a  vector  aoo  is  added  to  p  as  given  in  Eq.  (3.63), 
then  the  sum  also  satisfies  Eq.  (3.61).  It  is  then  apparent  that  Eq.  (3.61)  does  not 
determine  a  single  point  whose  normal  velocity  component  vanishes,  but  a  set  of 
points  lying  on  the  ISA,  and  thus,  other  solutions  are  possible.  Eor  example,  we  can 
find  the  point  of  the  ISA  lying  closest  to  the  origin.  To  this  end,  let  Pq  be  the  position 
vector  of  that  point.  This  vector  is  obviously  perpendicular  to  o),  i.e., 

=  0  (3.64) 

Next,  Eq.  (3.61)  is  rewritten  for  Pp,  and  Eq.  (3.64)  is  adjoined  to  it,  thereby  deriving 
an  expanded  linear  system  of  equations,  namely, 

ApJ)  =  b  (3.65) 


where  A  is  a  4  x  3  matrix  and  b  is  a  four-dimensional  vector,  both  being  given  below: 


+  (l/||a)|p)S^^a 
0 


(3.66) 


This  system  is  of  the  same  nature  as  that  appearing  in  Eq.  (3.10),  and  hence,  it  can 
be  solved  for  Pq  following  the  same  procedure.  Thus,  both  sides  of  Eq.  (3.65)  are 
multiplied  from  the  left  by  A^,  thereby  obtaining 

A^ApJ)  =  A^b  (3.67) 


where 


A^A  =  +  (0(0^  =  — +  (0(0^ 


(3.68) 


Moreover,  from  Eq.  (3.58),  the  rightmost  side  of  the  foregoing  relation  becomes 
||a)  Ipl,  and  hence,  the  matrix  coefficient  of  the  left-hand  side  of  Eq.  (3.67)  and  the 
right-hand  side  of  the  same  equation  reduce,  respectively,  to 


A^A  = 


(O 


1,  A^b  =  S^(a  —  S^a) 


(3.69) 


Upon  substitution  of  Eq.  (3.69)  into  Eq.  (3.67)  and  further  solving  for  Pq,  the  desired 
expression  is  derived: 


S^(a  —  S^a) 


\(o 


0)  X  (a  — 
lieu  I 


0)  X  a) 

2 


(3.70) 
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Thus,  the  instantaneous  screw  is  fully  defined  by  an  alternative  set  of  six 
independent  scalars,  namely,  the  three  components  of  its  angular  velocity  o)  and 
the  three  components  of  the  velocity  of  an  arbitrary  body  point  A,  denoted  by  a.  As 
in  the  case  of  the  screw  motion,  we  can  also  represent  the  instantaneous  screw  by  a 
line  and  two  additional  parameters,  as  we  explain  below. 


3.4.2  The  Twist  of  a  Rigid  Body 


A  line,  as  we  saw  earlier,  is  fully  defined  by  its  six-dimensional  Pliicker  array,  which 
contains  only  four  independent  components.  Now,  if  a  pitch  p  is  added  as  a  fifth 
feature  to  the  line  or,  correspondingly,  to  its  Pliicker  array,  we  obtain  a  screw  s, 
namely. 


s 


e 

p  X  e  + 


(3.71) 


An  amplitude  is  any  scalar  A  multiplying  the  foregoing  screw.  The  amplitude 
produces  a  twist  or  a  wrench,  to  be  discussed  presently,  depending  on  its  units.  The 
twist  or  the  wrench  thus  defined  can  be  regarded  as  an  eight-parameter  array.  These 
eight  parameters,  of  which  only  six  are  independent,  are  the  amplitude,  the  pitch, 
and  the  six  Pliicker  coordinates  of  the  associated  line.  Clearly,  a  twist  or  a  wrench 
is  defined  completely  by  six  independent  real  numbers.  More  generally,  a  twist  can 
be  regarded  as  a  six-dimensional  array  defining  completely  the  velocity  field  of  a 
rigid  body;  it  comprises  the  three  components  of  the  angular  velocity  and  the  three 
components  of  the  velocity  of  any  of  the  points  of  the  body. 

Below  we  elaborate  on  the  foregoing  concepts.  Upon  multiplication  of  the  screw 
appearing  in  Eq.  (3.71)  by  the  amplitude  A  representing  the  magnitude  of  an  angular 
velocity,  we  obtain  a  twist  t,  namely. 


_p  X  (Ae)  +  p{A^)_ 

where  the  product  Ae  can  be  readily  identified  as  the  angular  velocity  (o  parallel  to 
vector  e,  of  magnitude  |  A  | .  Moreover,  the  lower  part  of  t  can  be  readily  identified 
with  the  velocity  of  a  point  of  a  rigid  body.  Indeed,  if  we  regard  the  line  C  and  point 
O  as  sets  of  points  of  a  rigid  body  B  moving  with  an  angular  velocity  oo  and  such 
that  point  P  moves  with  a  velocity  po)  parallel  to  the  angular  velocity,  then  the 
lower  vector  of  t,  denoted  by  v,  represents  the  velocity  of  point  O,  i.e.. 


V  =  —CO  X  p  +  poo 
We  can  thus  express  the  twist  t  as 


V 


(3.72) 
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If  the  pitch  is  zero,  the  twist  is  a  pure  rotation;  if  infinite,  the  twist  is  a  pure 
translation,  in  which  case  the  twist  is 


(3.73) 


Then,  the  screw  of  infinite  pitch  Sqo  is  defined  as  the  six-dimensional  array  appearing 
in  the  above  equation,  namely. 


s 


CX) 


(3.74) 


Note  that  this  screw  array  is  identical  to  the  Plucker  array  of  a  line  at  infinity  lying 
in  a  plane  of  unit  normal  e,  as  displayed  in  Eq.  (3.33). 

The  twist  array,  as  defined  in  Eq.  (3.72),  with  (o  on  top,  represents  the  ray 
coordinates  of  the  twist.  An  exchange  of  the  order  of  the  two  Cartesian  vectors 
of  this  array,  in  turn,  gives  rise  to  the  axis  coordinates  of  the  twist. 

The  foregoing  twist  was  also  termed  motor  by  Everett  (1875).  As  Phillips  (1990) 
points  out,  the  word  motor  is  an  abbreviation  of  morntni  and  vector.  An  extensive 
introduction  into  motor  algebra  was  published  by  von  Mises  (1924),  a  work  that 
is  now  available  in  English  (von  Mises  1996).  Roth  (1984),  in  turn,  provided  a 
summary  of  these  concepts,  as  applicable  to  robotics.  The  foregoing  array  goes  also 
by  other  names,  such  as  the  German  Kinemate  or  the  Erench  torseur  cinematique. 

The  relationships  between  the  angular- velocity  vector  and  the  time  derivatives  of 
the  invariants  of  the  associated  rotation  are  linear.  Indeed,  let  the  three  sets  of  four 
invariants  of  rotation,  namely,  the  natural  invariants,  the  linear  invariants,  and  the 
Euler-Rodrigues  parameters  be  grouped  in  the  four-dimensional  arrays  v.  A,  and  rj, 
respectively,  i.e.. 


V 


(sin  0)e 
cosfi 


[sin(^/2)]e 
COS  (0/2) 


(3.75) 


We  then  have  the  linear  relations  derived  in  full  detail  elsewhere  (Angeles  1988), 
and  outlined  in  Appendix  A  for  quick  reference,  namely, 

V  =  N(o,  X  =  Lo),  rj  =  Hco  (3.76a) 

with  N,  L,  and  H  defined  as 


N  = 

L  = 

H  = 


[sin  0/(2(l  —  cos  0))]  (1  —  ee^)  —  (1/ 2)E 

■(l/2)[tr(Q)l-Q]' 

— (sin0)e^ 

1  cos(0  /2)1  —  sin(0  /  2)E 

2  —  sin(0/2)e^ 


(3.76b) 

(3.76c) 

(3.76d) 


where,  it  is  recalled,  tr(-)  denotes  the  trace  of  its  square  matrix  argument  (•),  i.e.,  the 
sum  of  the  diagonal  entries  of  that  matrix. 
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The  inverse  relations  of  those  shown  in  Eqs.  (3.76a)  are  to  be  derived  by  resorting 
to  the  approach  introduced  when  solving  Eq.  (3.65)  for  Pq,  thereby  obtaining 


(0  =Nv  =  Lx  =Hri  (3.77a) 

the  3x4  matrices  N,  L,  and  H  being  defined  below: 

N  =  [(sin  0)1  +  (1  —  cos0)E  e] ,  (3.77b) 

L  =  [l  +  [(sin0)/(l  +  cos0)]E  — [(sin0)/(l  +  cos0)]e] ,  (3.77c) 

H  =  2  [[cos(0/2)]l  +  [sin(0/2)]E  —  [sin(0/2)]e]  (3.77d) 


Caveat  The  angular  velocity  vector  is  not  a  time -derivative,  i.e.,  no  Cartesian  vector 
exists  whose  time -derivative  is  the  angular-velocity  vector. 

However,  matrices  N,  L,  and  H  of  Eqs.  (3.76b-d)  can  be  regarded  as  integration 
factors  that  yield  time-derivatives. 

Now  we  can  write  the  relationship  between  the  twist  and  the  time-rate  of  change 
of  the  seven-dimensional  pose  array  s,  namely, 

s  =  Tt  (3.78) 

where 

rp  ^  F  O43 

“[01 

in  which  O  and  O43  are  the  3x3  and  the  4x3  zero  matrices,  while  1  is  the  3x3 
identity  matrix  and  F  is,  correspondingly,  N,  L,  or  H,  depending  upon  the  invariant 
representation  chosen  for  the  rotation.  The  inverse  relationship  of  Eq.  (3.78)  takes 
the  form 


(3.79) 


where 


F  O 

O34  1 


(3.80a) 


(3.80b) 


in  which  O34  is  the  3x4  zero  matrix.  Moreover,  F  is  one  of  N,  L,  or  H,  depending 
on  the  rotation  representation  adopted,  namely,  the  natural  invariants,  the  linear 
invariants,  or  the  Euler-Rodrigues  parameters,  respectively. 

A  formula  that  relates  the  twist  of  the  same  rigid  body  at  two  different  points  is 
now  derived.  Let  A  and  P  be  two  arbitrary  points  of  a  rigid  body.  The  twist  at  each 
of  these  points  is  defined  as 


(O 


9 


(O 

\p 


(3.81) 
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Moreover,  Eq.  (3.50)  can  be  rewritten  as 

+  (a  —  p)  X  0)  (3.82) 

Combining  Eq.  (3.81)  with  Eq.  (3.82)  yields 


tp  — 


(3.83a) 


where 


1  O 
A-P  1 


(3.83b) 


with  the  6x6  matrix  U  defined  as  in  Eq.  (3.29b),  while  A  and  P  denote  the  cross- 
product  matrices  of  vectors  a  and  p,  respectively.  Thus,  Eqs.  (3.83a  and  b)  can  be 
fairly  called  the  twist-transfer  formulas. 
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Upon  differentiation  of  both  sides  of  Eq.  (3.49)  with  respect  to  time,  one  obtains 

p  =  a  +  ^  (p  —  a)  +  ^  (p  —  a)  (3.84) 

Now,  Eq.  (3.49)  is  solved  for  p  —  a,  and  the  expression  thus  resulting  is  substituted 
into  Eq.  (3.84),  thereby  obtaining 

p  =  a  +  (^  +  ^^)(p  —  a)  (3.85) 


where  the  matrix  sum  in  parentheses  is  termed  the  angular-acceleration  matrix  of 
the  rigid-body  motion  and  is  represented  by  W,  i.e., 

W  =  (3.86) 

Apparently,  the  first  term  of  the  right-hand  side  of  Eq.  (3.86)  is  skew- symmetric, 
whereas  the  second  one  is  symmetric.  Thus, 

vect(W)  =  vect(ft)  =  co  (3.87) 


o)  being  termed  the  angular-acceleration  vector  of  the  rigid-body  motion.  We  have 
now  an  interesting  result,  namely. 


tr(W)  =  tr(S^^)  =  tr(— ||a)||^l  +  (ooo^) 


=  — ||a)  ll^tr(l)  -\-  (o  ot)  =  —2\\(o 


(3.88) 
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Moreover,  Eq.  (3.85)  can  be  written  as 

p  =  a  +  o)  X  (p  —  a)  +  0)  X  [o)  X  (p  —  a)]  (3.89) 

On  the  other  hand,  the  time  derivative  of  t,  henceforth  referred  to  as  the  twist  rate, 
is  displayed  below: 


(3.90) 


in  which  v  is  the  acceleration  of  a  point  of  the  body.  The  relationship  between  the 
twist  rate  and  the  second  time  derivative  of  the  screw  is  derived  by  differentiation  of 
both  sides  of  Eq.  (3.78),  which  yields 


s  =  Tt  +  tt 


where 


F  O43 
O  O 


(3.91) 


(3.92) 


and  F  is  one  of  N,  L,  or  H,  accordingly.  The  inverse  relationship  of  Eq.  (3.91)  is 
derived  by  differentiating  both  sides  of  Eq.  (3.80a)  with  respect  to  time,  which  yields 


t  =  Ss  +  Ss 


where 


F  O 

O34  O 


(3.93) 


(3.94) 


with  O  and  O34  already  defined  in  Eq.  (3.80b)  as  the  3x3  and  the  3x4  zero 

•  •  •  • 

matrices,  respectively,  while  F  is  one  of  N,  L,  or  H,  according  with  the  type  of 
rotation  representation  at  hand. 

Before  we  embark  in  differentiating  the  foregoing  matrices,  we  introduce  a  few 
definitions:  Let 


i.e. 


(3.95a) 


^0 


cos 


u  =  sin^e,  uq  =  cos^, 


r  =  sm 


(3.95b) 
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Thus,  the  time  derivatives  sought  take  on  the  forms 


1 

4(1  —  COS0) 


(l/2)[ltr(Q)-Q] 

-(l/2)«^[ltr(Q)-Qn. 

—{(o  •  u)l  —  (1/2)S^Q 
-(l/2)a)^[ltr(Q)-Qn_ 


1 

H  =  - 
2 


rol  -  R 


(3.96a) 


(3.96b) 

(3.96c) 


where  we  have  used  the  identities  below,  which  are  derived  in  Appendix  A. 


tr(Q)  =  tr(S^Q)  = 


(3.96d) 


Furthermore,  R  denotes  the  cross-product  matrix  of  r,  and  B  is  defined  as 

B  =  — 2(e  •  (o)l  +  2(3  —  cos0)(e  •  o))ee^  —  2(1  +  sin(p)(oe^ 

— (2cos0  +  sin^)eo)^  —  (sin0)[S^  —  (e  •  a))E]  (3.96e) 


Moreover, 


N  =  [0 (cos  0)1  +  0(sin0)E  e] 


(3.97a) 


L  =  [Y/D  u]  (3.97b) 

H  =  [rol  +  R  -r]  (3.97c) 

where  V  and  D  are  defined  below: 

V  =  U  -  (uu^  +  uu^)  -  ^(U  -  uu^)  (3.97d) 

D  =  I  uq  (3.97e) 

with  U  denoting,  now,  the  cross-product  matrix  of  u. 
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3.6  Rigid-Body  Motion  Referred  to  Moving  Coordinate  Axes 

Although  in  kinematics  no  “preferred”  coordinate  system  exists,  in  dynamics  the 
governing  equations  of  rigid-body  motions  are  valid  only  in  inertial  frames,  the 
concept  to  be  defined  in  Sect.  3.8.  Thus,  it  is  important  to  refer  vectors  and  matrices 
to  inertial  frames,  but  sometimes  it  is  not  possible  to  do  so  directly.  For  instance, 
a  space  vessel  can  be  supplied  with  instruments  to  measure  the  velocity  and  the 
acceleration  of  a  satellite  drifting  in  space,  but  the  measurements  taken  from  the 
space  vessel  will  be  referred  to  a  coordinate  frame  fixed  to  it,  which  is  not  inertial. 
If  the  motion  of  the  vessel  with  respect  to  an  inertial  coordinate  frame  is  recorded, 
e.g.,  from  an  Earth-based  station,  then  the  acceleration  of  the  satellite  with  respect 
to  an  inertial  frame  can  be  computed  using  the  foregoing  information.  How  to  do 
this  is  the  subject  of  this  section. 

In  the  realm  of  kinematics,  it  is  not  necessary  to  distinguish  between  inertial 
and  noninertial  coordinate  frames,  and  hence,  it  will  suffice  to  call  the  coordinate 
systems  involved  fixed  and  moving.  Thus,  consider  the  fixed  coordinate  frame 
X,  Y,  Z,  which  will  be  labeled  T,  and  the  moving  coordinate  frame  X,  y,  Z,  which 
will  be  labeled  M.,  both  being  depicted  in  Fig.  3.7.  Moreover,  let  Q  be  the  rotation 
matrix  taking  frame  T  into  the  orientation  of  J\A,  and  o  the  position  vector  of  the 
origin  of  J\A  from  the  origin  of  T.  Further,  let  p  be  the  position  vector  of  point  P 
from  the  origin  of  T  and  p  the  position  vector  of  the  same  point  from  the  origin  of 
fiA.  From  Fig.  3.7  one  has 


[p]t  =  [o]j^  +  [p]t  (3.98) 

where  it  will  be  assumed  that  p  is  not  available  in  frame  T,  but  in  fiA .  Hence, 

[p]t  =  [Q]j^[/>]m  (3.99) 


Fig.  3.7  Fixed  and  moving  coordinate  frames 
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Substitution  of  Eq.  (3.99)  into  Eq.  (3.98)  yields 

[p]j^  =  [o]jr  +  [Q]jr[p]x  (3.100) 

Now,  in  order  to  compute  the  velocity  of  P,  both  sides  of  Eq.  (3.100)  are 
differentiated  with  respect  to  time,  which  leads  to 

Iv]t  =  [o]jr  +  [Q]y^[/)]M  +  [Q]j^[/>]m  (3.101) 

Eurthermore,  from  the  definition  of  Eq.  (3.44),  we  have 

[Q]j^  =  (3.102) 

upon  substitution  of  the  foregoing  relation  into  Eq.  (3.101),  we  obtain 

[P]j^  =  [o]jr  +  ]jr[Q]jr[p]>(  +  [Q]jr[/>]>(  (3.103) 

which  is  an  expression  for  the  velocity  of  P  in  in  terms  of  the  velocity  of  P  in  M. 
and  the  twist  of  M  with  respect  to  T.  Next,  the  acceleration  of  E*  in  is  derived 
by  differentiation  of  both  sides  of  Eq.  (3.103)  with  respect  to  time,  which  yields 

[p]t  =  [o]jr  +  ]t[Q]t[p]m 

+  [^  ]jr[Q]jr[/)]M  +  [Q]j^[/>]a4  +  [Q]j^[p]a4  (3.104) 

Eurther,  upon  substitution  of  identity  (3.102)  into  Eq.  (3.104),  we  obtain 

[p]j^  =  [o]j^  +  ([^  ]j^  +  [^^]j^)[Q]j^[p]a( 

+  2[^  ]jr[Q]jr[/>]x  +  [Q]jr[p]>(  (3.105) 

Moreover,  from  the  results  of  Sect.  3.5,  it  is  apparent  that  the  first  two  terms  of 
the  right-hand  side  of  Eq.  (3.105)  represent  the  acceleration  of  E*  as  a  point  of  M., 
whereas  the  fourth  term  is  the  acceleration  of  P  measured  from  A4 .  The  third  term 
is  known  as  the  Coriolis  acceleration,  for  it  was  first  pointed  out  by  the  Erench 
mathematician  Gustave  Gaspard  Coriolis  (1835). 


3.7  Static  Analysis  of  Rigid  Bodies 

Germane  to  the  velocity  analysis  of  rigid  bodies  is  their  force- and-moment  analysis. 
In  fact,  striking  similarities  exist  between  the  velocity  relations  associated  with  rigid 
bodies  and  the  forces  and  moments  acting  on  them.  Erom  elementary  statics  it  is 
known  that  the  resultant  of  all  external  actions,  i.e.,  forces  and  moments,  exerted 
on  a  rigid  body  can  be  reduced  to  a  force  f  acting  at  a  point,  say  A,  and  a  moment 
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Fig.  3.8  Equivalent  systems  of  force  and  moment  acting  on  a  rigid  body 


nq.  Alternatively,  the  same  force  f  can  be  defined  as  acting  at  an  arbitrary  point 
P  of  the  body,  as  depicted  in  Fig.  3.8,  but  then  the  resultant  moment  Up  changes 
correspondingly. 

In  order  to  establish  a  relationship  between  nq  and  n p ,  the  moment  of  the  first 
system  of  force  and  moment  with  respect  to  point  P  is  equated  to  the  moment  about 
the  same  point  of  the  second  system,  thus  obtaining 

np  =  nq  +  (a  —  p)  X  f  (3.106) 

which  can  be  rewritten  as 

np  =  nq  +  f  X  (p  —  a)  (3.107) 

whence  the  analogy  with  Eq.  (3.50)  is  apparent.  Indeed,  and  nq  of  Eq.  (3.107) 
play  the  role  of  the  velocities  of  P  and  A,  p  and  a,  respectively,  whereas  f  of 
Eq.  (3.107)  plays  the  role  of  co  of  Eq.  (3.50).  Thus,  similar  to  Theorem  3.4.2, 
one  has 

Theorem  3.7.1.  For  a  given  system  offerees  and  moments  acting  on  a  rigid  body,  if 
the  resultant  force  is  applied  at  any  point  of  a  particular  line  C!' ,  then  the  resultant 
moment  is  of  minimum  magnitude.  Moreover,  that  minimum-magnitude  moment  is 
parallel  to  the  resultant  force. 

Hence,  the  resultant  of  the  system  of  forces  and  moments  is  equivalent  to  a 
force  f  acting  at  a  point  of  C"  and  a  moment  n,  with  both  f  and  n  parallel  to  C" . 
Paraphrasing  the  definition  of  the  ISA,  one  defines  line  C"  as  the  axis  of  the  wrench 
acting  on  the  body.  Let  no  be  the  minimum-magnitude  moment.  Paraphrasing 
Eq.  (3.52)  in  turn,  no  can  be  expressed  as 


f  n/5  •  f 


no  =  ^0 


(3.108) 
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Moreover,  the  pitch  of  the  wrench,  p",  is  defined  as 


P 


// 


Up  •  f 


or 


Inup  • f 


(3.109) 


which,  again,  has  units  of  m/rad  or,  correspondingly,  of  m/turn.  Of  course,  the 
wrench  axis  can  be  defined  by  its  Pliicker  array,  p/://,  i.e.. 


VjC" 


=  p  X  e 


// 


(3.110) 


where  e"  is  the  unit  vector  parallel  to  C",  n"  is  the  moment  of  C"  about  the  origin, 
and  p  is  the  position  vector  of  any  point  on  C". 

The  wrench  axis  is  fully  specified,  then,  by  the  direction  of  f  and  point  Pq  of 
position  vector  Pq  lying  closest  to  the  origin,  which  can  be  derived  by  analogy  with 
Eq.  (3.70),  namely,  as 


2^  X  (n^ 


f  X  a) 


Similar  to  Theorem  3.4.1,  one  has 


(3.111) 


Theorem  3.7.2.  Consider  a  system  of  moments  and  forces  acting  on  a  rigid  body, 
with  the  resultant  force  applied  at  an  arbitrary  point  of  the  body.  The  projection  of 
the  resultant  moment  onto  the  wrench  axis  is  identical  for  all  points. 

From  the  foregoing  discussion,  then,  the  wrench  applied  to  a  rigid  body  can 
be  fully  specified  by  the  resultant  force  f  acting  at  an  arbitrary  point  P  and 
the  associated  moment,  Up.  We  shall  derive  presently  the  counterpart  of  the  six¬ 
dimensional  array  of  the  twist,  namely,  the  wrench  array.  Upon  multiplication  of 
the  screw  of  Eq.  (3.71)  by  an  amplitude  A  with  units  of  force,  what  we  will  obtain 
would  be  a  wrench  w,  i.e.,  a  six-dimensional  array  with  its  first  three  components 
having  units  of  force  and  its  last  components  units  of  moment.  We  would  like  to 
be  able  to  obtain  the  power  developed  by  the  wrench  on  the  body  moving  with  the 
twist  t  by  a  simple  inner  product  of  the  two  arrays.  However,  because  of  the  form  the 
wrench  w  has  taken,  the  inner  product  of  these  two  arrays  would  be  meaningless, 
for  it  would  involve  the  sum  of  two  scalar  quantities  with  different  units;  moreover, 
none  of  the  two  quantities  has  an  immediate  physical  meaning.  In  fact,  the  first 
scalar  would  have  units  of  force  by  frequency  (angular  velocity  by  force),  while  the 
second  would  have  units  of  moment  of  moment  multiplied  by  frequency  (velocity 
by  moment),  thereby  leading  to  a  physically  meaningless  result.  This  inconsistency 
can  be  resolved  if  we  redefine  the  wrench  not  simply  as  the  product  of  a  screw  by  an 
amplitude,  but  as  a  linear  transformation  of  that  screw  involving  the  6x6  array  T 
defined  as 


O  1 
1  O 


(3.112) 
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where  O  and  1  denote,  respectively,  the  3x3  zero  and  identity  matrices.  Now  we 
define  the  wrench  as  a  linear  transformation  of  the  screw  s  defined  in  Eq.  (3.71).  This 
transformation  is  obtained  upon  multiplying  s  by  the  product  AT ,  the  amplitude  A 
having  units  of  force,  i.e.. 


w  =  AT  s  = 


p  X  (^e)  +  p(Ae) 
Ae 


The  foregoing  wrench  is  given  in  axis  coordinates,  while  the  twist  was  given  in  ray 
coordinates. 

Now,  the  first  three  components  of  the  foregoing  array  can  be  readily  identified 
as  the  moment  of  a  force  of  magnitude  A  acting  along  a  line  of  action  given  by  the 
Pliicker  array  of  Eq.  (3.1 10),  with  respect  to  a  point  P,  to  which  a  moment  parallel 
to  that  line  and  of  magnitude  pA  is  added.  Moreover,  the  last  three  components  of 
that  array  pertain  apparently  to  a  force  of  magnitude  A  and  parallel  to  the  same  line. 
We  denote  here  the  above-mentioned  moment  by  n  and  the  force  by  f,  i.e., 

f  =  Ae,  n  =  p  X  f  +  pf 
The  wrench  w  is  then  defined  as 


w  = 


(3.113) 


which  can  thus  be  interpreted  as  a  representation  of  a  system  of  forces  and  moments 
acting  on  a  rigid  body,  with  the  force  acting  at  point  P  of  the  body  B  defined  above 
and  a  moment  n.  Under  these  circumstances,  we  say  that  w  acts  at  point  P  of  B. 

With  the  foregoing  definitions  it  is  now  apparent  that  the  wrench  has  been  defined 
so  that  the  inner  product  t^w  will  produce  the  power  Ft  developed  by  w  acting  at  P 
when  B  moves  with  a  twist  t  defined  at  the  same  point,  i.e., 

n=Vw  (3.114) 


When  a  wrench  w  that  acts  on  a  rigid  body  moving  with  the  twist  t  develops  zero 
power  onto  the  body,  we  say  that  the  wrench  and  the  twist  are  reciprocal  to  each 
other.  By  the  same  token,  the  screws  associated  with  that  wrench-twist  pair  are  said 
to  be  reciprocal.  More  specifically,  let  the  wrench  and  the  twist  be  given  in  terms  of 
their  respective  screws,  and  s^,  as 

w=WTs^,  t=Tst,  (3.115) 

where  W  and  T  are  the  amplitudes  of  the  wrench  and  the  twist,  respectively,  while 
r  is  as  defined  in  Eq.  (3.112).  Thus,  the  two  screws  and  St  are  reciprocal  if 


(rs„)^s,  =  =  0 


(3.116) 
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and  by  virtue  of  the  symmetry  of  F,  the  foregoing  relation  can  be  further 
expressed  as 

s^rs^=0  or  s[Ysyv  =  0  (3.117) 

The  product  s J  F  St  or  its  equivalent  sf  F  is  termed  the  reciprocal  product  of  the 

two  screws  and  s^. 

Now,  if  A  and  P  are  arbitrary  points  of  a  rigid  body,  we  define  the  wrench  at 
these  points  as 


= 


Therefore,  Eq.  (3.106)  leads  to 


wp  =  Vw^ 


where 


1  A-P 
0  1 


(3.118) 


(3.119a) 


(3.119b) 


with  A  and  P  already  defined  in  Eq.  (3.83b)  as  the  cross-product  matrices  of  vectors 
a  and  p,  respectively.  Thus,  Wp  is  a.  linear  transformation  of  w^.  By  analogy  with 
the  twist-transfer  formulas  of  Eqs.  (3.83a  and  b),  Eqs.  (3. 1 19a  and  b)  are  termed  here 
the  wrench-transfer  formulas. 

Multiplying  the  transpose  of  Eq.  (3.83a)  by  Eq.  (3.119a)  yields 

=  t^U^Vw^  (3.120) 


where 


1  -A  +  P" 

1  A  -  P' 

0  1 

0  1 

(3.121) 


with  16x6  denoting  the  6x6  identity  matrix.  Thus,  =  t^w^,  as  expected, 

since  the  wrench  develops  the  same  amount  of  power,  regardless  of  where  the  force 
is  assumed  to  be  applied.  Also  note  that  an  interesting  relation  between  U  and  V 
follows  from  Eq.  (3.121),  namely. 


(3.122) 


3.8  Dynamics  of  Rigid  Bodies 

The  equations  governing  the  motion  of  rigid  bodies  are  recalled  in  this  section  and 
cast  into  a  form  suitable  to  multibody  dynamics.  A  rigid  body  B  of  mass  density  p, 
which  need  not  be  uniform,  is  assumed  at  the  outset.  It  is  further  assumed  that  the 
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body  is  under  motion,  with  angular  velocity  co .  The  kinetic  energy  of  B  is  defined 
as  the  integral  of  the  elementary  kinetic  energy  of  a  differential  element  dB  of  the 
body  over  the  whole  body.  The  mass  of  the  body  is  thus  given  by 

m  =  f  pdB  (3.123) 

JB 

B  denoting  also  the  region  of  the  three-dimensional  space  occupied  by  the  body. 
Given  that  the  laws  of  classical  mechanics — as  exemplified  by  the  Newton-Euler 
equations,  to  be  recalled  presently — hold  only  in  inertial  frames,  the  concept  is  now 
introduced: 

An  inertial  frame  is  a  coordinate  system  that  translates  with  uniform  velocity  and  constant 
orientation  with  respect  to  the  stars. 

Now,  if  p  denotes  the  position  vector  of  an  arbitrary  point  P  of  the  body,  from 
the  origin  O  of  a  given  inertial  frame,  the  mass  first  moment  of  the  body  with  respect 
to  O,  qo,  is  defined  as 

qo=f  PVdB  (3.124) 

JB 

Upon  recalling  the  Mean-value  Theorem  of  integral  calculus  (Brand  1965),  there 
exists  a  point  C ,  of  position  vector  c,  such  that 


=  me 


point  C  thus  being  defined  as  the  center  of  mass  of  B.  That  is. 


c  = 


m 


(3.125) 


Notice  that  point  C  need  not  be  located  within  the  physical  boundary  of  B — 
think,  for  example,  of  a  homogeneous  torus. 

Further,  the  mass  second  moment  of  the  body  with  respect  to  O  is  introduced. 
To  this  end,  the  kinetic  energy  of  the  rigid  body,  denoted  by  T,  is  defined  as  the 
integral  of  the  kinetic  energy  of  the  mass  element  dm  =  pdB  of  the  body  over  the 
whole  body,  i.e.. 


r  =  /  (3.126) 

where  p  is  the  velocity  of  P  and  p  is  the  position  vector  of  P  in  the  same  inertial 
frame,  whose  origin,  for  the  sake  of  simplicity,  is  assumed  to  be  stationary.  Next, 
the  velocity  of  P  is  expressed  as  in  Eq.  (3.50),  using  a  reference — or  base — point;  in 
that  equation,  an  arbitrary  point  A  was  used  as  a  reference.  It  will  prove  convenient 
to  use  the  center  of  mass  C  as  reference  in  the  sequel.  Thus, 


p  =  c  +  a)xr,  r  =  p  —  c 


(3.127) 
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or,  if  matrix  R  =  CPM(r)  is  introduced,  then 

p  =  c-R(o  (3.128) 


Upon  substitution  of  the  above  expression  into  Eq.  (3.126),  an  alternative  expres¬ 
sion  for  the  kinetic  energy  is  obtained: 

T  =  -  f  p[||cf  -  2c^R<y  +  (o^R^R(o]dB 
2  Jb 

The  foregoing  expression  is  now  rewritten  as  the  sum  of  three  integrals: 


m 


Now,  since  R  is  skew-symmetric,  its  square  expands  according  to  Eq.  (2.40), 
namely. 


R^  =  —  I  rpl  +  rr 


.T 


Eurthermore,  the  integral  appearing  in  the  second  term  of  Eq.  (3.129)  vanishes. 
Indeed, 


f  pRdB=  f 
Jb  Jb 


pRdB=  /  pCFM(r)dB  =  CFM(  prdB) 


L 


where  the  integral  and  the  CPM(-)  operators  have  been  interchanged  because 
they  are  both  linear  and  smooth.^  Eurther,  if  the  definition  of  r  is  recalled  from 
Eq.  (3.127),  the  above  integral  expands  to 


which  indeed  vanishes.  Therefore,  the  kinetic  energy  of  B  reduces  to 


1 


T  =  -ml  cl  ^  H — 
2  2 


^  JB 


(3.130) 


ic 


^Smoothness  means  that  they  do  not  introduce  jump  discontinuities  upon  differentiation. 
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which  is  known  to  be  a  positive-definite  quantity,  in  that  it  (a)  is  positive  and  (b) 
only  vanishes  when  the  body  is  stationary  in  an  inertial  frame,  i.e.,  when  both  c 
and  (o  vanish.  In  fact,  the  first  term  of  the  above  expression  is  apparently  positive- 
definite,  for  its  first  factor,  the  mass,  is  positive,  its  second  factor  being  the  square 
of  a  real  quantity,  namely,  a  Euclidean  norm — which  is  itself  positive.  The  second 
term  is  a  quadratic  form  in  the  angular-velocity  vector,  associated  with  matrix 
Ic ,  defined  as  the  moment  of  inertia  of  B  at  the  center  of  mass.  As  this  is  a 
matrix,  Ic  is  also  referred  to  as  the  moment- of -inertia  matrix  .  This  matrix  is  the 
integral  of  the  quantity  in  parentheses  that  appears  multiplied  by  the  density  p, 
an  inherently  positive  quantity.  The  term  in  parentheses,  however,  is  apparently 

'T' 

positive-semidefinite,  as  it  equals  the  product  RR  ;  this  product  would  be  positive- 
definite,  were  it  not  for  the  cross-product  matrix  R,  which  is  known  to  be  singular, 
of  rank  2 — see  Excercise  2.11.  Therefore,  Ic  is  at  least  positive-semidefinite.  As  a 
matter  of  fact,  the  only  case  in  which  Ic  is  semidefinite  occurs  when  B  is  one¬ 
dimensional,  i.e.,  a  “long”  body  with  two  of  its  dimensions  negligible  with  respect 
to  its  length.  This  would  be  a  body  filling  the  space  of  a  line  segment,  its  moment-of- 
inertia  matrix  having  a  nullity — dimension  of  its  null  space — of  1 ,  with  null  space 
spanned  by  a  vector  parallel  to  the  line.  Other  than  this  rather  unlikely  case,  Ic  is 
positive-definite. 

Therefore,  Ic  has  three  real  eigenvalues,  in  general  all  three  positive,  known 
as  the  principal  moments  of  inertia,  and  three  mutually  orthogonal  eigenvectors, 
defining  three  principal  axes  of  inertia  of  B,  all  three  concurring  at  the  center  of 
mass. 

Note,  however,  that  the  principal  moments  and  the  principal  axes  of  inertia  of 
a  rigid  body  depend  on  the  point  with  respect  to  which  the  moment  of  inertia  is 
defined.  Indeed,  the  moment  of  inertia  can  be  defined  about  an  arbitrary  point  of 
the  body,  whether  within  its  physical  boundary  or  without.  If,  in  Eq.  (3.126),  the 
moment  of  inertia  is  taken  with  respect  to  an  arbitrary  point  A  of  B,  then,  the 
moment  of  inertia  1a  with  respect  to  A  is  defined  as 


1  -  (p  -  a)(p 


aY]dB 


(3.131) 


Next,  notice  that,  with  the  definition  of  r  in  Eq.  (3.127),  p  —  a  can  be  expressed  as 


p  —  a  =  r—  (a  —  c) 


(3.132) 


whence  1a  becomes 


c)fl-[r-(a 


c)][r-  (a 


c)f}dB 


which  expands  to 
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-L 


=  I  p(||r||^l  —  +2  (—  /  pdB) (a  —  c)l (a  —  c)  I  pdB 


L 


T 


L 


J 


Ic 


0^^ 


" - V - " 


0^ 


L 


+  (/  rpdB)(a  — c)^ I  p[||a  —  c||^l  —  (a  —  c)(a  —  c)^ 


L 


J- 


^ - V - " 


0 


m[||a— c|pl— (a— c)(a— c)^] 


the  final  expression  for  thus  being 


Ia  =  lc  ^[||a  -  c||^l  -  (a  -  c)(a  -  c)^] 


(3.133a) 


which,  in  light  of  Eq.  (2.39),  can  be  cast  in  the  form 


Ia  =  Ic+  m(A  -  C)(A  -  C) 


T 


(3.133b) 


a  result  known  as  Steiner’s  Theorem,  a.k.a.  the  Theorem  of  Parallel  Axes  . 

Next,  the  Newton-Euler  equations  governing  the  motion  of  a  rigid  body  are 
recalled,  with  the  body  B  acted  upon  by  a  wrench  of  force  f  applied  at  its  center 
of  mass,  and  of  moment  nc  about  the  center  of  mass.  The  Newton  equation  then 
takes  the  form 


f  =  me 


(3.134a) 


whereas  the  Euler  equation  is 


nc  =  +  0)  X  Icco  (3.134b) 

The  momentum  m  and  the  angular  momentum  he  of  a  rigid  body  moving  with  a 
velocity  of  its  center  of  mass  c  and  an  angular  velocity  oo  are  defined  below,  the 
angular  momentum  being  defined,  moreover,  with  respect  to  the  center  of  mass. 
These  momenta  are 


m  =  me,  he  =  (3.135) 

Furthermore,  the  time-derivatives  of  the  foregoing  quantities  are  readily  computed 
(see  Exercise  3.17)  as 


m  =  me,  he  =  x  \c(o 

and  hence,  Eqs.  (3.134a  and  b)  take  the  forms 


(3.136) 


f  =  m,  nc  =  he 


(3.137) 
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The  set  of  equations  (3.134a  and  b)  are  known  as  the  Newton-Euler  equations  . 
These  can  be  written  in  a  more  compact  form  as  described  below.  First,  a  6  x  6 
matrix  M  is  introduced  that,  following  von  Mises  (1924),  is  termed  here  the  inertia 
dyad,  namely. 


M  = 


Ic  O 
O  ml 


(3.138) 


where  O  and  1  denote  the  3x3  zero  and  identity  matrices.  A  similar  6x6  matrix  was 
defined  by  von  Mises  under  the  above  name.  However,  von  Mises ’s  inertia  dyad  is 
full,  while  the  matrix  defined  above  is  block-diagonal.  Both  matrices,  nevertheless, 
denote  the  same  physical  property  of  a  rigid  body,  i.e.,  its  mass  and  moment  of 
inertia.  Now  the  Newton-Euler  equations  can  be  written  as 

Mt  +  WMt  =  w  (3.139) 


in  which  matrix  W,  henceforth  termed  the  angular-velocity  dyad,  by  similarity  with 
the  inertia  dyad,  is  defined  in  turn  as 


9.  O 
O  O 


(3.140) 


with  9  already  defined  as  the  angular- velocity  matrix,  namely,  as  CPM(a)).  Note 
that  the  twist  of  a  rigid  body  lies  in  the  null  space  of  its  angular- velocity  dyad,  i.e., 

Wt  =  0  (3.141) 


Further  definitions  are  introduced  below:  the  momentum  screw  of  the  rigid  body 
about  the  center  of  mass  is  the  six-dimensional  vector  jjl  defined  as 

=  Mt  (3.142) 

Moreover,  from  Eqs.  (3.136)  and  definition  (3.142),  the  time-derivative  of  jjl  can  be 
readily  derived  as^ 


=  Mt  +  W/^  =  Mt  +  WMt  (3.143) 

The  kinetic  energy  of  a  rigid  body  undergoing  a  motion  in  which  its  center  of 
mass  moves  with  velocity  c  and  rotates  with  an  angular  velocity  (o  is  given  by 
Eq.  (3.130),  repeated  below  for  quick  reference: 


^See  Exercise  3.17. 


(3.144) 
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From  the  foregoing  definitions,  then,  the  kinetic  energy  can  now  be  written  as  a 
quadratic  form  of  the  body  twist,  namely. 


1  r 

T  =  -t^Mt 

2 


(3.145) 


while  the  Newton-Euler  equations  can  be  written  in  a  form  more  compact  than  that 
of  Eq.  (3.139): 


A 


=  w 


(3.146) 


which  is  a  six-dimensional  vector  equation. 

Properly  speaking,  M,  as  given  by  Eq.  (3.138),  should  be  subscripted  with  C,  to 
emphasize  that  the  moment  of  inertia  in  the  upper  left  block  of  M  is  taken  with 
respect  to  the  center  of  mass.  For  brevity,  we  will  dispense  with  this  subscript 
whenever  the  moment  of  inertia  is  centroidal.  If  the  moment  of  inertia  is  taken 
with  respect  to  any  other  arbitrary  point  A,  then  the  corresponding  inertia  dyad  is 
represented  with  .  The  reader  is  asked  to  verify  that  takes  the  form 


Ia  m(C-A) 
m(C  — A)^  ml 


(3.147) 


Notice  that,  contrary  to  the  inertia  dyad  defined  with  respect  to  the  center  of  mass 
in  Eq.  (3.138),  which  is  block- diagonal,  is  full.  It  should  not  be  too  difficult 

to  prove  that,  regardless  of  the  point  with  respect  to  which  the  moment  of  inertia 
is  defined,  the  inertia  dyad  is  positive-definite — see  Exercise  3.20.  The  proof,  as 
suggested  in  that  exercise,  hinges  on  a  decomposition  of  in  the  form 

=  Me  +  mPP^  (3.148) 


The  reader  is  invited  to  find  an  expression  for  P.  The  foregoing  expression  for 
can  be  fairly  referred  to  as  Steiner’s  Theorem  in  dyad  form. 


3.9  Exercises 

3.1  The  cube  of  Fig.  3.9  is  displaced  from  configuration  AB  . . .  H  into  configura¬ 
tion  A'B'  ...H'. 

(a)  Determine  the  matrix  representing  the  rotation  Q  undergone  by  the  cube, 
in  X,  Y,  Z  coordinates. 

(b)  Find  the  Pliicker  coordinates  of  line  C  of  the  cube  undergoing  displace¬ 
ments  of  minimum  magnitude. 

(c)  Find  the  intersections  of  C  with  the  coordinate  planes. 
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D 


Fig.  3.9  Motion  of  a  cube 

Fig.  3.10  A  regular 
tetrahedron 


3.2  Two  unit  forces,  fi  and  f2,  are  applied  to  the  regular  tetrahedron  of  unit-length 
edges  displayed  in  Fig.  3.10  in  such  a  way  that  fi  is  directed  from  P2  to 
whereas  f2  is  directed  from  P^^  to  Pi.  The  effect  of  the  foregoing  system  of 
forces  on  the  rigid  tetrahedron  is  obtained  by  application  of  the  resultant  of 
the  two  forces  on  a  certain  point  P  and  a  moment  n.  Find  the  location  of  point 
P  lying  closest  to  P^  that  will  make  the  magnitude  of  n  a  minimum. 


3.9  Exercises 


133 


3.3  The  moment  of  a  line  Ci  about  a  second  line  £2  is  a  scalar  /x  defined  as 


/X  =  ni  •  02 

where  ni  is  the  moment  of  Ci  about  an  arbitrary  point  P  of  £2,  while  02 
is  a  unit  vector  parallel  to  line  £2.  Apparently,  the  necessary  and  sufficient 
condition  for  two  lines  to  intersect  is  that  the  moment  of  one  about  the  other 
vanish. 

Using  the  above  concept,  show  that  the  locus  of  all  lines  £  intersecting 
three  given  lines  {Ck  is  a  quadric,  i.e.,  a  surface  defined  by  a  function 
that  is  quadratic  in  the  position  vector  p  of  a  point  of  the  surface:  / (p)  = 
p^Mp+g^p+/z  =  0,  h  >  0.  Notice  that,  the  surface  being  generated  by  the 
motion  of  a  line  constrained  to  touch  the  three  given  lines,  the  surface  is  bound 
to  be  ruled.  Now,  there  are  only  two  ruled,  second-order  surfaces  (Pottmann 
and  Wallner  2001),  the  one- sheet  hyperboloid  and  the  hyperbolic  paraboloid. 
Geometrically,  the  former  is  a  closed  surface,  the  latter  open.  Algebraically, 
the  former  is  characterized  by  a  matrix  coefficient  M  with  det(M)  <  0, 
meaning  that  the  matrix  has  two  positive  and  one  negative  eigenvalues.  The 
hyperbolic  paraboloid  is  characterized  by  a  singular  M,  i.e.,  det(M)  =  0. 
Show,  furthermore,  that  det(M)  is  nonzero — showing  that  det(M)  <  0  is  far 
more  challenging! — and  hence,  the  quadric  is  a  one-sheet  hyperboloid.  Hint: 
Deriving  the  given  expression  for  f  (p)  should  be  done  without  resorting  to 
components.  Showing  that  M  is  singular  requires  resorting  to  components. 
To  this  end,  choose  the  coordinate  axes  appropriately  so  as  to  avoid  too 
cumbersome  expressions. 

3.4  A  robotic  gripper  is  provided  with  two  redundant  sensors  that  are  meant  to 
measure  a  wrench  acting  on  the  gripper.  The  ith  sensor,  moreover,  has  its 
own  coordinate  frame,  labeled  Tt,  for  i  =  1,2.  Sensor  i  reported  the  ith 
measurement  of  the  wrench  wp,  where  subscript  P  indicates  that  the  force  is 
applied  at  point  P,  as  [  w/?  ]/  =  [n^,  ,  for  /  =  1,2.  These  measurements 

are  given  as 


1 

0 

1 _ 

0 

1 _ 

II 

c 

0 

_5_ 

,  [f]i  = 

1 

(N  0 

_ 1 

■  -5/3  ■ 

■-4/3" 

[n]2  = 

-10/3 

,  [f]2  = 

4/3 

_  10/3  _ 

_  2/3  _ 

(a)  Show  that  the  measurements  are  compatible,  based  on  invariance  argu¬ 
ments. 

(b)  Determine  the  relative  orientation  of  the  two  frames,  i.e.,  find  the  rotation 
matrix  transforming  P2 -coordinates  into  P’1 -coordinates. 
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Fig.  3.11  A  workpiece  B  to  be  grasped  by  a  gripper  Q 


3.5  A  robot-calibration  method  has  been  proposed  that  allows  us  to  determine 
the  location  of  a  joint  axis,  C,  via  the  Pliicker  coordinates  of  the  axis  in  a 
coordinate  frame  fixed  to  the  gripper.  The  Pliicker  coordinates  are  given  as 

jrc  =  [e^,n^V. 

(a)  Show  that  the  distance  of  the  axis  to  the  origin  of  the  gripper-fixed 
coordinate  frame,  d,  can  be  determined  SiS  d  =  ||n||. 

(b)  Show  that  the  point  P*  on  the  axis,  which  lies  closest  to  the  above- 
mentioned  origin,  has  a  position  vector  p*  given  as 

p*  =  e  X  n 

(c)  From  measurements  on  a  robot,  the  Pliicker  coordinates  were  estimated, 
in  a  gripper- fixed  frame  Q,  as 


[y^c]g  =  [-V2/2,  0,  V2/2,  0,  -V2,  0]^ 


Find  d  and  p*  in  gripper  coordinates 

3.6  The  gripper  ^  of  a  robot  is  approaching  a  workpiece  B,  as  indicated  in 
Fig.  3.11,  with  planes  TJi  and  772  parallel  to  each  other  and  normal  to  TJ^. 
The  workpiece  is  made  out  of  a  cube  of  unit  length  from  which  two  vertices 
have  been  removed,  thereby  producing  the  equilateral  triangular  faces  DBF 
and  D'E' F'.  Moreover,  two  coordinate  frames,  T  (X,  Y,  Z)  and  J^'  (X',  T', 
Z'),  are  defined  as  indicated  in  the  figure,  in  which  Y  is,  apparently,  parallel 
to  line  D'C' . 
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It  is  required  to  grasp  B  with  Q  in  such  a  way  that  planes  TJi  and  772 
coincide  with  the  triangular  faces,  while  carrying  the  Y'  axis  to  an  orientation 
perpendicular  to  the  diagonal  CC'  of  B.  More  concretely,  in  the  grasping 
configuration,  frame  T'  is  carried  into  J^"  (X",  7",  and  Z"),  not  shown  in 
the  figure,  in  such  a  way  that  unit  vectors  i",  j",  k",  parallel  to  X" ,  Y" ,  Z", 
respectively,  are  oriented  so  that  \"  has  all  three  of  its  J^-components  positive, 
while  j"  has  its  Z-component  positive. 

(a)  Compute  the  angle  of  rotation  of  the  motion  undergone  by  Q  from  a  pose 
in  which  T'  and  T  have  identical  orientations,  termed  the  reference  pose, 
and  find  the  unit  vector  parallel  to  the  axis  of  rotation,  in  frame  J^. 

(b)  The  position  vector  of  point  7*  of  ^  is  known  to  be,  in  the  reference  pose. 


2 

-1 

0.25 


Determine  the  set  of  points  of  Q  undergoing  a  displacement  of  minimum 
magnitude,  under  the  condition  that  P ,  in  the  displaced  configuration  of 
Q,  coincides  with  C' . 


3.7  In  calibrating  a  robot,  the  Pliicker  coordinates  of  one  of  its  axes  are  to  be 
determined  in  a  given  coordinate  frame.  To  this  end,  the  moment  of  this  axis 
is  measured  with  respect  to  two  points,  A  and  B,  of  position  vectors  [a]  = 
[1,  0,  0]^  and  [b]  =  [0,  1,  1]^,  respectively.  The  said  moments,  andn^, 
respectively,  are  measured  as 


0 

"O' 

[^a]  = 

1 

(N  O 

_ 1 

-  [«£]  = 

1 

_1_ 

with  all  entries  given  in  meters. 

(a)  Determine  the  unit  vector  e  defining  the  direction  of  the  axis  under 
discussion. 

(b)  Find  the  coordinates  of  the  point  P*  of  the  axis  that  lies  closest  to  the 
origin 

(c)  Find  the  Pliicker  coordinates  of  the  axis  about  the  origin,  i.e.,  the  Pliicker 
coordinates  of  the  axis  in  which  the  moment  is  defined  with  respect  to  the 
origin. 

3.8  Prove  that  for  any  three-dimensional  vectors  oo  and  p. 


(x)  X  (co  X  •  •  •  (co  X  (co  xp))  •  •  • )  —  ( —  1)^  (  (O 


2k 


(0 


(0(0  )p 


2k  factors 


0)  X  (o)  X  •  •  •  (o)  X  (o)  xp))  •  •  •)  =  (—lY(\\(o\\^^(o)  X  p 


2k 1  factors 
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3.9  A  “small”  rotation  is  defined  as  that  about  an  arbitrary  axis  parallel  to  the 
unit  vector  e,  through  a  “small”  angle  0,  so  that  0^1.  Prove  that  the 
angular- velocity  vector,  in  the  special  case  of  “small”  rotations,  turns  out  to  be 
a  time-derivative.  What  is  the  vector  whose  time-derivative  yields  the  angular- 
velocity  vector? 

3.10  Derive  an  expression  for  the  angular  velocity  (o  in  terms  of  Euler  angles, 
which  were  introduced  in  Exercise  2.19.  More  specifically,  if  we  store  the 
Euler  angles  in  array  rj  =  [9,  0,  0  ]^,  then,  find  the  matrix  W  such  that 

(o  =Wyi 

Notice  that,  given  rj  and  o),  an  expression  for  rj  can  be  obtained  upon  inverting 
W.  However,  W  is  not  always  invertible.  Eind  under  which  conditions  W 
becomes  singular.  Notice:  The  use  of  computer  algebra  is  strongly  recom¬ 
mended  to  solve  this  exercise. 

3.11  A  rectangular  prism  with  regular  hexagonal  bases  whose  sides  are  25  mm 
long  and  whose  height  is  150  mm  is  to  undergo  a  pick-and-place  operation — 
see  Chap.  6  to  understand  what  this  means — that  requires  knowledge  of 
its  centroid  location  and  its  moment-of-inertia  matrix.  Eind  the  centroidal 
principal  axes  and  moments  of  inertia  under  the  assumption  that  the  prism 
is  made  from  a  homogeneous  material. 

3.12  The  prism  of  Exercise  3.11  now  undergoes  a  machining  process  cutting  it  into 
two  parts,  which  are  separated  by  a  plane  that  contains  one  of  the  edges  of  the 
base  and  makes  an  angle  of  45°  with  the  axis  of  the  prism.  Eind  the  centroidal 
principal  axes  and  moments  of  inertia  of  each  of  the  two  parts. 

3.13  In  Exercise  2.23  assume  that  a  mass  m  is  located  at  every  point  Pi  of  position 
vector  p, .  Give  a  mechanical  interpretation  of  the  matrix  m[tr(PP^)l  —  PP^], 
with  P  defined  in  that  exercise. 

3.14  The  centroidal  inertia  matrix  of  a  rigid  body  is  measured  by  two  observers, 
who  report  the  two  results  below: 


[lU 


"1  0  o' 

, ,  1 

'6  2  2' 

020 

.  [I]b  =  T 

2  5  0 

_0  0  3_ 

_2  0  7_ 

Show  that  the  two  measurements  are  acceptable.  Hint:  Use  invariance  argu¬ 
ments. 

3.15  State  the  conditions  under  which  a  point  and  the  center  of  mass  of  a  rigid 
body  share  the  same  principal  axes  of  inertia.  In  other  words,  let  Ip  and  Ic  be 
the  moment-of-inertia  matrices  of  a  rigid  body  about  a  point  P  and  its  center 
of  mass,  C ,  respectively.  State  the  conditions  under  which  the  two  matrices 
have  common  eigenvectors.  Moreover,  under  these  conditions,  what  are  the 
relationships  between  the  two  sets  of  principal  moments  of  inertia? 
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3.16  Show  that  the  smallest  principal  moment  of  inertia  of  a  rigid  body  attains  its 
minimum  value  at  the  center  of  mass. 

3.17  Show  that  the  time-rate  of  change  of  the  inertia  dyad  M  of  a  rigid  body  is 
given  by 


M  =  WM  -  MW 


Then,  recall  the  momentum  screw  jjl  defined  as 

IJi  =  Mt 

where  t  is  the  twist  of  the  body,  defined  at  its  center  of  mass.  Now,  with  the 
above  expression  for  M,  restate  the  result  displayed  in  Eq.  (3.143),  i.e.,  show 
that 

ft  =  Mi  +  WMt 

3.18  A  classical  result  in  the  dynamics  of  robots  establishes  a  relation  between  the 
time-derivative  of  the  n  y.  n  inertia  matrix  i(«)  of  a  serial  robot  and  what 
is  known  as  the  “Coriolis  and  centrifugal  forces”  matrix  C(0,0),  where  0 

is  the  w -dimensional  vector  of  joint  angles — these  concepts  are  the  subject 

•  • 

of  Chap.  7.  According  to  this  relation,  the  difference  I(^)  —  2C(0 ,0)  is  skew- 
symmetric.  Show  that  the  same  relation  holds  for  one  single  rigid  body  moving 
in  space  under  the  action  of  a  given  wrench.  That  is,  show  that  the  difference 
M  —  2WM  is  skew-symmetric,  with  M  and  W  introduced  in  Sect.  3.8. 

3.19  A  wrench  w  =  [  ]^,  with  f  acting  at  point  P  of  the  gripper  of  Fig.  2.10, 

is  measured  by  a  six-axis  force  sensor,  to  which  a  frame  P's  is  attached,  as 
indicated  in  that  figure.  If  points  P  and  S  lie  a  distance  of  100  mm  apart,  find 
the  wrench  in  p2^  when  the  readouts  of  the  sensor  are 


"r 

"o' 

[n]s  = 

0 

_i_ 

Nm, 

[f]s  = 

1 

_0_ 

N 


3.20  Derive  Eq.  (3.147).  Moreover,  show  that  M^,  as  given  in  that  equation,  is 
positive-definite — Hint:  show  that  can  be  expressed  as  =  Me  + 
PP^,  with  Me  positive -definite  and  PP^  positive- semidefinite.  The  above 
expression  can  be  termed  the  6x6  form  of  Steiner’s  Theorem,  a.k.a.  the 
Parallel- Axis  Theorem.  Now,  in  trying  to  mimic  Steiner’s  Theorem,  one  would 
like  to  express  P  in  the  same  form  as  the  second  term  of  Eq.  (3.133a),  as  the 
product  mRR^,  with  R  defined  as  a  suitable  6x6  matrix.  Can  this  matrix  be 
found?  If  yes,  give  an  expression  for  R;  if  not,  explain  why. 

3.21  The  inner  (dot)  product  of  two  m  x  n  matrices  A  and  B  can  be  defined  as 
tr(AB^),  which  leads  to  the  definition  of  the  angle  0  between  A  and  B  as 
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cosO  =  tr(AB^)/||A||||B||,  where  ||  A||  =  ytr(AA^)  with  a  similar  definition 
for  II B II .  Moreover,  Eq.  (3.86)  can  be  regarded  as  the  Cartesian  decomposition 
of  the  angular-acceleration  matrix  W.  Show  that  the  two  components  of  the 
foregoing  decomposition  are  mutually  orthogonal,  which  is  a  restatement  of 
the  orthogonality  of  the  tangential  and  the  centripetal  relative  acceleration  of 
two  points  of  a  rigid  body. 


Chapter  4 

Geometry  of  Decoupled  Serial  Robots 


4.1  Introduction 

This  chapter  is  devoted  to  the  displacement  analysis  of  robotic  manipulators  of  the 
serial  type,  which  we  call  the  geometry  of  serial  robots.  The  study  is  limited  to 
decoupled  robots,  to  be  defined  below,  the  inverse  displacement  analysis  of  general 
six-axis  robots  being  the  subject  of  Chap.  9.  These  robots  serving  mainly  to  perform 
manipulation  tasks,  they  are  also  referred  to  as  manipulators. 

We  begin  by  defining  a  serial,  /i-axis  manipulator.  In  connection  with  this 
manipulator,  additionally,  we  will  (a)  introduce  the  Denavit-Hartenberg  notation 
for  the  definition  of  link  frames  that  uniquely  determine  the  architecture  and  the 
configuration,  or  posture,  of  the  manipulator  at  hand;  (b)  define  the  Cartesian  and 
joint  coordinates  of  this  manipulator;  and  (c)  relate  these  two  sets  of  variables  by 
means  of  its  geometric  model.  Moreover,  with  regard  to  six-axis  manipulators,  we 
will  define  decoupled  manipulators  and  provide  a  procedure  for  the  solution  of  their 
inverse  displacement  model. 


4.2  The  Denavit-Hartenberg  Notation 

One  of  the  first  tasks  of  a  robotics  engineer  is  the  geometric  modeling  of  a  robotic 
manipulator.  This  task  consists  in  devising  a  model  that  can  be  unambiguously  (a) 
described  to  a  control  unit  through  a  database  and  (b)  interpreted  by  other  robotics 
engineers.  The  purpose  of  this  task  is  to  give  manipulating  instructions  to  a  robot, 
regardless  of  the  dynamics  of  the  manipulated  load  and  the  robot  itself.  The  simplest 
way  of  geometrically  modeling  a  robotic  manipulator  is  by  means  of  the  concept  of 
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Fig.  4.1  The  two  basic  lower  kinematic  pairs:  the  revolute  and  the  prismatic  joints 

kinematic  chain.  A  kinematic  chain  is  a  set  of  rigid  bodies,  also  called  links,  coupled 
by  kinematic  pairs,  also  termed  joints.  A  kinematic  pair  is,  then,  the  coupling  of  two 
rigid  bodies  so  as  to  constrain  their  relative  motion.  We  distinguish  two  basic  types 
of  kinematic  pairs,  namely,  higher  and  lower  kinematic  pairs.  A  higher  kinematic 
pair  arises  between  rigid  bodies  when  contact  takes  place  along  a  line  or  at  a  point. 
This  type  of  coupling  occurs  in  cam-and- follower  mechanisms,  gear  trains,  and 
roller  bearings,  for  example.  A  lower  kinematic  pair  occurs  when  contact  takes  place 
along  a  surface  common  to  the  two  bodies.  Six  different  types  of  lower  kinematic 
pairs  can  be  distinguished  (Angeles  1982;  Hartenberg  and  Denavit  1964),  but  all 
these  can  be  produced  from  two  basic  types,  namely,  the  rotating  pair,  denoted  by 
R  and  also  called  revolute,  and  the  sliding  pair,  represented  by  P  and  also  called 
prismatic. 

The  common  surface  along  which  contact  takes  place  in  a  revolute  pair  is 
commonly  billed  as  a  cylinder.  However,  a  cylinder  exhibits  two  kinds  of  symmetry, 
of  revolution  and  of  extrusion,  a  cylindrical  surface  thus  allowing  for  both  rotation 
about  the  axis  of  the  cylinder  and  translation  along  a  direction  parallel  to  the  axis. 
For  this  reason,  an  axially  symmetric  surface  devoid  of  symmetry  of  extrusion  was 
proposed  by  Khan  and  Angeles  (2011).  A  typical  realization  of  the  revolute  joint  is 
the  coupling  through  journal  bearings.  Thus,  two  rigid  bodies  coupled  by  a  revolute 
can  rotate  relative  to  each  other  about  the  axis  of  the  common  cylinder,  which  is  thus 
referred  to  as  the  axis  of  the  revolute,  but  are  prevented  from  undergoing  relative 
translations  as  well  as  rotations  about  axes  other  than  the  cylinder  axis.  On  the 
other  hand,  the  common  surface  of  contact  between  two  rigid  bodies  coupled  by 
a  prismatic  pair  is  a  prism  of  arbitrary  cross  section,  and  hence,  the  two  bodies 
coupled  in  this  way  are  prevented  from  undergoing  any  relative  rotation  and  can 
move  only  in  a  pure-translation  motion  along  a  direction  parallel  to  the  axis  of  the 
prism.  As  an  example  of  this  kinematic  pair,  one  can  cite  the  dovetail  coupling.  Note 
that  whereas  the  revolute  axis  is  a  totally  defined  line  in  three-dimensional  space, 
the  prismatic  pair  has  no  axis;  this  pair  has  only  a  direction.  That  is,  the  prismatic 
pair  does  not  have  a  particular  location  in  space.  Nevertheless,  and  for  the  sake  of 
conciseness,  we  will  refer  to  joint  axis  generically,  when  speaking  of  either  revolute 
or  prismatic  joints.  Bodies  coupled  by  a  revolute  and  a  prismatic  pair  are  shown  in 
Fig.  4.1. 
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Serial  manipulators  will  be  considered  in  this  chapter,  their  associated  kinematic 
chains  thus  being  of  the  simple  type,  i.e.,  each  and  every  link  is  coupled  to  at  most 
two  other  links.  A  simple  kinematic  chain  can  be  either  closed  or  open.  It  is  closed 
if  each  and  every  link  is  coupled  to  two  other  links,  the  chain  then  being  called  a 
linkage;  it  is  open  if  it  contains  exactly  two  links,  the  end  ones,  that  are  coupled  to 
only  one  other  link.  Thus,  simple  kinematic  chains  studied  in  this  chapter  are  open, 
and  in  the  particular  robotics  terminology,  their  first  link  is  called  the  manipulator 
base,  whereas  their  last  link  is  termed  the  end-effector  (EE). 

Thus,  the  kinematic  chains  associated  with  manipulators  of  the  serial  type  are 
composed  of  binary  links,  the  intermediate  ones,  and  exactly  two  simple  links,  those 
at  the  ends.  Hence,  except  for  the  end  links,  all  links  carry  two  kinematic  pairs, 
and  as  a  consequence,  two  pair  axes — just  remember  that  a  prismatic  pair  has  a 
direction  but  no  axis.  In  order  to  uniquely  describe  the  architecture  of  a  kinematic 
chain,  i.e.,  the  relative  location  and  orientation  of  its  neighboring  pair  axes,  the 
Denavit-Hartenberg  notation  (Denavit  and  Hartenberg  1955)  is  introduced.  To  this 
end,  links  are  numbered  0,  1,  . . . ,  n,  the  /th  pair  being  defined  as  that  coupling  the 
(i  —  l)st  link  with  the  ith  link.  Hence,  the  manipulator  is  assumed  to  be  composed 
of  n  I  links  and  n  pairs;  each  of  the  latter  can  be  either  R  or  P,  where  link  0  is  the 
fixed  base,  while  link  n  is  the  end-effector.  Next,  a  coordinate  frame  is  defined 
with  origin  Of  and  axes  Xt ,  Yf ,  Zi .  This  frame  is  attached  to  the  (i  —  l)st  link — not 
to  the  ith  link! — for  i  =  I,  ...,  n  1.  This  is  the  classical  Denavit-Hartenberg 
notation.  Khalil  and  collaborators  (Khalil  and  Dombre  2002)  modified  this  notation 
to  make  it  “less  ambiguous.”  In  the  balance  of  the  book  we  follow  the  classical 
notation.  For  the  first  n  frames,  this  is  done  following  the  rules  given  below: 

1 .  Zi  is  the  axis  of  the  i  th  pair.  Notice  that  there  are  two  possibilities  of  defining  the 
positive  direction  of  this  axis,  since  each  pair  axis  is  only  a  line,  not  a  directed 
segment.  Moreover,  the  Zf  axis  of  a  prismatic  pair  can  be  located  arbitrarily, 
since  only  its  direction  is  defined. 

2.  Xi  is  defined  as  the  common  perpendicular  to  Zf-i  and  Z/,  directed  from  the 
former  to  the  latter,  as  shown  in  Fig.  4.2a.  Notice  that  if  these  two  axes  intersect, 
the  positive  direction  of  Xf  is  undefined  and  hence,  can  be  freely  assigned. 
Henceforth,  we  will  follow  the  right-hand  rule  in  this  case.  This  means  that  if 
unit  vectors  i/,  k/_i,  and  k/  are  attached  to  axes  Xi,  Z/_i,  and  Z/,  respectively, 
as  indicated  in  Fig. 4.2b,  then  i/  is  defined  as  k;_i  x  k/.  Moreover,  if  Z/_i  and 
Zi  are  parallel,  the  location  of  Xi  is  undefined.  In  order  to  define  it  uniquely,  we 
will  specify  Xi  as  passing  through  the  origin  of  the  (/  —  l)st  frame,  as  shown  in 
Fig.  4.2c. 

3.  The  distance  between  Zi  and  Z^+i  is  defined  as  ai,  which  is  thus  nonnegative. 

4.  The  Z; -coordinate  of  the  intersection  Oj  of  Zi  with  is  denoted  by  bi. 
Since  this  quantity  is  a  coordinate,  it  can  be  either  positive  or  negative.  Its 
absolute  value  is  the  distance  between  Xi  and  +i ,  also  called  the  offset  between 
successive  common  perpendiculars  to  the  corresponding  joint  axes. 

5.  The  angle  between  Zi  and  Z/+i  is  defined  as  ai  and  is  measured  about  the 

positive  direction  of  This  item  is  known  as  the  twist  angle  between 

successive  pair  axes. 
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b 


1 


X  kj 


Fig.  4.2  Definition  of  Xi  when  Z/_i  and  Z/ :  (a)  are  skew;  (b)  intersect;  and  (c)  are  parallel 


6.  The  angle  between  Xf  and  is  defined  as  Of  and  is  measured  about  the 

positive  direction  of  Zf . 

The  (n  +  l)st  coordinate  frame  is  attached  to  the  far  end  of  the  nth  link.  Since  the 
manipulator  has  no  (n  +  l)st  link,  the  foregoing  rules  do  not  apply  to  the  definition  of 
this  frame.  The  analyst,  thus,  has  the  freedom  to  define  this  frame  as  it  best  suits  the 
task  at  hand.  Notice  that  /7  + 1  frames,  •  •  •,  have  been  defined,  whereas 

links  are  numbered  from  0  to  /i.  In  summary,  a  /7-axis  manipulator  is  composed  of 
/7  +  1  links  and  /7  +  1  coordinate  frames.  These  rules  are  illustrated  with  an  example 
below. 

Consider  the  architecture  depicted  in  Fig.  4.3,  usually  referred  to  as  a  Puma 
robot,  which  shows  seven  links,  numbered  from  0  to  6,  and  seven  coordinate  frames, 
numbered  from  1  to  7.  Note  that  the  last  frame  is  arbitrarily  defined,  but  its  origin  is 
placed  at  a  specific  point  of  the  EE,  namely,  at  the  operation  point  P ,  which  is  used 
to  define  the  task  at  hand.  Furthermore,  three  axes  intersect  at  a  point  C ,  and  hence, 
all  points  of  the  last  three  links  move  on  concentric  spheres  with  respect  to  7^4, 
for  which  reason  the  subchain  comprising  these  three  links  is  known  as  a  spherical 
wrist,  point  C  being  its  center.  By  the  same  token,  the  subchain  composed  of  the 
first  four  links  is  called  the  arm.  Thus,  the  wrist  is  decoupled  from  the  arm,  and  is 
used  for  orientation  purposes,  the  arm  being  used  for  the  positioning  of  point  C. 
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Fig.  4.3  Coordinate  frames  of  a  Puma  robot 


The  arm  is  sometimes  called  the  regional  structure  and  the  wrist  the  local  structure, 
the  overall  manipulator  thus  being  of  the  decoupled  type. 

In  the  foregoing  discussion,  if  the  i  th  pair  is  R,  then  all  quantities  involved  in 
those  definitions  are  constant,  except  for  Of ,  which  is  variable  and  is  thus  termed 
the  joint  variable  of  the  /th  pair.  The  other  quantities,  i.e.,  at,  bi,  and  at,  are  the 
joint  parameters  of  the  same  pair.  If,  alternatively,  the  /th  pair  is  P,  then  bi  is 
variable,  and  the  other  quantities  are  constant.  In  this  case,  the  joint  variable  is 
bi ,  and  the  joint  parameters  are  ai ,  ai ,  and  Oi .  Notice  that  associated  with  each 
joint  there  are  exactly  one  joint  variable  and  three  constant  parameters.  Hence, 
a  /7-axis  manipulator  has  n  joint  variables — which  are  henceforth  grouped  in  the 
/7 -dimensional  vector  0 ,  regardless  of  whether  the  joint  variables  are  angular  or 
translational — and  3/7  constant  parameters.  The  latter  define  the  architecture  of  the 
manipulator,  while  the  former  determine  its  configuration,  or  posture. 

Whereas  the  manipulator  architecture  is  fully  defined  by  its  3/7  Denavit- 
Hartenberg  (DH)  parameters,  its  posture  is  fully  defined  by  its  n  joint  variables, 
also  called  its  joint  coordinates,  once  the  DH  parameters  are  known.  The  relative 
pose — position  and  orientation — between  links  is  fully  specified,  then,  from  the 
background  of  Chap.  2,  by  (a)  the  rotation  matrix  taking  the  Xi ,  Yi ,  Zi  axes  into  a 
configuration  in  which  they  are  parallel  pairwise  to  the  X/+i,  T/+i,  Z/+i  axes,  and 
(b)  the  position  vector  of  the  origin  of  the  latter  in  the  former.  The  representations 
of  the  foregoing  items  in  coordinate  frame  Ti  will  be  discussed  presently.  First,  we 
obtain  the  matrix  representation  of  the  rotation  Q,  carrying  2^  into  an  orientation 
coincident  with  that  of  assuming,  without  loss  of  generality  because  we  are 
interested  only  in  changes  of  orientation,  that  the  two  origins  are  coincident,  as 
depicted  in  Fig.  4.4.  This  matrix  is  most  easily  derived  if  the  rotation  of  interest  is 
decomposed  into  two  successive  rotations,  as  indicated  in  Fig.  4.5.  In  that  figure, 
X[,  Y' ,  Z-  is  an  intermediate  coordinate  frame  jF[,  obtained  by  rotating  Ti  about 
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Fig.  4.4  Relative  orientation 
of  the  zth  and  (/  +  l)st 
coordinate  frames 


i 


K- 


i  +  1 


n 


Fig.  4.5  (a)  Rotation  about  axis  Z/  through  an  angle  Oi ;  and  (b)  relative  orientation  of  the  i  ^th 

and  the  (/  +  l)st  coordinate  frames 


the  Zi  axis  through  an  angle  9i.  Then,  the  intermediate  frame  is  rotated  about  X// 
through  an  angle  at ,  which  takes  it  into  a  configuration  coincident  with  +i .  Let  the 
foregoing  rotations  be  denoted  by  [  C/  ]/  and  [A/]//,  respectively,  which  are  readily 
derived  for  they  are  in  the  canonical  forms  (2.56c)  and  (2.56a),  respectively. 

Moreover,  let 


Xi  =  cos  at,  jjii  =  sin  O'/  (4.1a) 

One  thus  has,  using  subscripted  brackets  as  introduced  in  Sect.  2.2, 


cos  Oi  —  sin  Oi  0 

1  0  0 

[C,],  = 

sin  Oi  cos  Oi  0 

> 

II 

1 

o 

1 

o 

o 

_ 1 

1 

o 

1 _ 

(4.1b) 
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and  hence,  the  matrix  sought  is  computed  simply  as 

[Q,]/ =  [C,],[A,V  (4.1c) 

Henceforth,  we  will  use  the  abbreviations  introduced  below: 


(4.  Id) 


thereby  doing  away  with  brackets,  when  these  are  self-understood.  Thus, 


Qi  ^  [Q/  ]/  ^ 


cos  Oi  —Xi  sin  Of  {Zi  sin  Oi 
sin  Oi  Xi  cos  Oi  —jjii  cos  Oi 

0  fZi  Xi 


(4.1e) 


One  more  factoring  of  matrix  Qi ,  which  will  be  used  in  Chap.  9,  is  given  below: 


(4.2a) 


with  X,  and  Z,  defined  as  two  pure  reflections,  the  former  about  the  F,  Z/  plane,  the 
latter  about  the  Xi  Yi  plane,  namely. 


1  0  0 

COS  Oi  sin  Oi  0 

X;  ^ 

1 

o 

,  z,-  ^ 

sin  Oi  —  cos  Oi  0 

1 

o 
_ 1 

o 

o 

_ 1 

(4.2b) 


Note  that  both  X/  and  Z/  are  symmetric  and  self-inverse — see  Sect.  2.2.  In  order 
to  derive  an  expression  for  the  position  vector  a/  connecting  the  origin  Oi  of  2^  with 
that  of  T'i+i,  0/+1,  reference  is  made  to  Fig.  4.6,  showing  the  relative  positions  of 
the  different  origins  and  axes  involved.  From  this  figure,  apparently, 

=  OiOi^i  =  OiO-  -h  O'-Oi^i  (4.3a) 


where  obviously. 


[OiO]]i  = 

"O' 

^  [0'iOi^i]i^i  — 

Oi 

0 

0 

UJ 

_0_ 

Now,  in  order  to  compute  the  sum  appearing  in  Eq.  (4.3a),  the  two  foregoing 
vectors  should  be  expressed  in  the  same  coordinate  frame,  namely,  2^ .  Thus, 


[0;6>,+i],-  =[Q,H6>;6>,+i],+i  = 


Oi  cos  Oi 
Oi  sin  Oi 
0 
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and  hence, 


Qi  COS  Of 

at  sin  Oi 

_  bi  _ 


For  brevity,  we  introduce  one  more  definition: 


(4.3b) 


(4.3c) 


Similar  to  the  foregoing  factoring  of  Q/ ,  vector  a/  admits  the  factoring 


a/ 


where  b,  is  given  by 


(4.3d) 


(4.3e) 


with  the  definitions  introduced  in  Eq.  (4.  la).  Hence,  vector  b,  is  constant  for  revolute 
pairs.  From  the  geometry  of  Fig.  4.6,  it  should  be  apparent  that  b,  is  nothing  but  a, 
in  J^+i,  i.e.. 


—  [az]z  -i-i  • 


Matrices  Q/  can  also  be  regarded  as  coordinate  transformations.  Indeed,  let  i/ ,  j/ , 
and  k;  be  the  unit  vectors  parallel  to  the  Xi ,  Yi ,  and  Z/  axes,  respectively,  directed 
in  the  positive  direction  of  these  axes.  From  Fig.  4.6,  it  is  apparent  that 


cos  Oi 

jXi  sin  Oi 

[h+i]/  — 

sin  Oi 

’  [  +  l  \i  — 

-jii  cos  Oi 

_  0  _ 

_  A,-  _ 

whence 


[j;  +  l  ]i  —  [k;+i  X  ];■  — 


—Xi  sin  Oi 
Xi  cos  Oi 


Therefore,  the  components  of  i/+i,  j/+i,  and  k/+i  in  are  nothing  but  the 
first,  second,  and  third  columns  of  Q/.  In  general,  then,  any  vector  v  in  is 
transformed  into  J^i  in  the  form 
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Fig.  4.6  Layout  of  three  successive  coordinate  frames 

[v];  =  [Q/],[v],+i 

which  is  a  similarity  transformation,  as  defined  in  Eq.  (2.118).  Likewise,  any  matrix 
M  in  is  transformed  into  Ti  by  the  corresponding  similarity  transformation,  as 
given  by  Eq.  (2.128): 

[M],-  =[Q,MM],+i[Qn,- 

The  inverse  relations  follow  immediately  in  the  form 

[  V  ],+i  =  [  Qf  [  V  ],• ,  [  M  ],+i  =  [  Qf  \i  [  M  ],■  [  Qi  ],• 


or,  upon  recalling  the  first  of  definitions  (4.  Id), 

[vL-  =Q,[v],+i,  [M],-  =Q,[M],+iQf  (4.4a) 

[vL+i  =  [M],+i  =  Qf  [M],Q,-  (4.4b) 

Moreover,  if  we  have  a  chain  of  i  frames,  T\,T2,  >  ^  then  the  inward  coordinate 
transformation  from  to  is  given  by 

[v]i  =  QiQ2---Qi-i[v]i  (4.5a) 

[M]i  =QiQ2---Q,-i[M],(QiQ2---Q,-i)^  (4.5b) 
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Fig.  4.7  The  data  sheet  of  the  FANUC  LRMate  200/  C  robot,  with  all  dimensions  in  mm:  (a)  top 
view;  (b)  orthographic  projection;  (c)  side  view;  and  (d)  front  view 


Likewise,  the  outward  coordinate  transformation  takes  the  form 

[v],-  =  (QiQ2---Q,-i)^[v]i  (4.6a) 

[M],.  =  (QiQ2---Q,-i)^[M]iQiQ2---Q,-i  (4.6b) 

Example  4.2.1.  The  data  sheet  of  the  six-axis  FANUC  LR  Mate  200/ C  robot  is 
displayed  in  Fig.  4.7.  Define  seven  coordinate  frames,  according  with  the  Denavit- 
Hartenberg  notation,  with  IFi  fixed  to  the  base  and  Tq  to  the  EE.  Then,  produce  a 
table  containing  the  1 8  (constant)  Denavit-Hartenberg  parameters  of  the  robot. 

Solution:  First,  the  seven  Z^-axes  are  identified.  Going  from  the  base  upwards,  Zi 
is  vertical,  while  Z2  and  Z3  are  horizontal,  300  mm  apart,  as  per  the  side  view,  Zi 
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Fig.  4.8  The  kinematic  chain 
of  the  FANUC  LRMate 
200/  C  robot  at  an  arbitrary 
posture 


lying  75  mm  from  the  vertical  plane  defined  by  Z2  and  Z3.  Z4  appears  horizontal  in 
Fig.  4.7,  75  mm  above  Z3,  but  it  can  attain  other  orientations.  Moreover,  Z5  is  also 
displayed  horizontal  in  Fig.  4.7  parallel  to  Z2  and  Z3;  however,  since  Z4  turns  the 
link  to  which  Z5  is  fixed,  the  one  with  the  robot  name  painted  in  the  orthographic 
projection,  Z5,  in  general,  is  not  parallel  to  the  two  forgoing  axes.  Furthermore,  in 
the  same  figure,  Z6,  the  axis  of  symmetry  of  the  end  plate,  to  which  the  EE  is  rigidly 
attached,  is  shown  coincident  with  Z4,  but  in  general,  these  two  axes  intersect, 
together  with  Z5,  at  the  point  located  in  the  side  view  80  mm  from  the  plane  of  the 
end  plate  and  (330  +  300  +  75  =)705  mm  from  the  plane  of  the  base.  The  point  at 
which  Z4,  Z5  and  Z6  intersect  is  the  center  C  of  the  wrist.  To  ease  the  definition  of 
the  DH  parameters,  the  robot  kinematic  chain  is  displayed  in  Fig.  4.8  in  an  arbitrary 
posture.  With  the  Z/  axes,  for  /  =  1 , . . . ,  6,  identified,  Z7  is  defined  as  coinciding 
with  Z6  for  simplicity,  as  this  axis  is  not  bound  by  the  DH  notation.  Likewise,  Xi 
can  be  defined  arbitrarily,  as  long  as  it  (a)  is  horizontal  and  (b)  intersects  Zi .  Finally, 
X7  can  also  be  defined  arbitrarily,  as  long  as  it  intersects  Z7  at  right  angles.  In  the 
next  step,  the  X^-axes  are  defined  according  with  the  DH  notation.  Once  all  14  axes 
are  defined,  the  definition  of  the  DH  parameters  is  straightforward:  the  ai  parameters 
are  simple,  bt  are  less  so,  but  their  determination  is  eased  once  the  0[  intersections 
have  been  identified.  These  parameters  are  listed  in  Table  4.1. 
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Table  4.1  TheDH 
parameters  of  the  FANUC 
LRMate  200/  C  robot 


/ 

Qi  (mm) 

bi  (mm) 

«,(°) 

1 

75 

330 

-90 

2 

300 

0 

0 

3 

75 

0 

-90 

4 

0 

320 

90 

5 

0 

0 

90 

6 

0 

80 

0 

4.3  The  Geometric  Model  of  Six-Revolute  Manipulators 

The  kinematics  of  serial  manipulators  begins  with  the  study  of  the  geometric 
relations  between  joint  variables  and  Cartesian  variables.  The  former  were  defined 
in  Sect.  4.2  as  those  determining  the  posture  of  a  given  manipulator,  with  one  such 
variable  per  joint;  a  six-axis  manipulator,  like  the  one  displayed  in  Fig.  4.9,  thus  has 
six  joint  variables,  Oi,  O2,  . . 0^.  The  Cartesian  variables  of  a  manipulator,  in  turn, 
are  those  variables  defining  the  pose  of  the  EE;  since  six  independent  variables  are 
needed  to  define  the  pose  of  a  rigid  body,  the  manipulator  of  Fig.  4.9  thus  involves 
six  Cartesian  variables. 

The  study  outlined  above  pertains  to  the  geometry  of  the  manipulator,  for  it 
involves  one  single  pose  of  the  EE.  Besides  geometry,  the  kinematics  of  manipu¬ 
lators  comprises  the  study  of  the  relations  between  the  time-rates  of  change  of  the 
joint  variables,  referred  to  as  the  joint  rates,  and  the  twist  of  the  EE.  Additionally, 
the  relations  between  the  second  time-derivatives  of  the  joint  variables,  referred  to 
as  the  joint  accelerations,  with  the  time-rate  of  change  of  the  twist  of  the  EE  also 
pertain  to  robot  kinematics. 

In  the  balance  of  this  chapter  we  study  the  geometry  of  manipulators,  the 
relations  between  joint  rates,  joint  accelerations  and  their  Cartesian  counterparts, 
twist  and  twist-rate,  being  the  subject  of  Chap.  5.  In  this  regard,  we  distinguish 
two  problems,  commonly  referred  to  as  the  direct  and  the  inverse  displacement 


Fig.  4.9  Serial  six-axis  manipulator 
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problems,  or  DDP  and  correspondingly,  IDP,  for  brevity.  In  the  DDP,  the  six  joint 
variables  of  a  given  six-axis  manipulator  are  assumed  to  be  known,  the  problem 
consisting  in  finding  the  pose  of  the  EE.  In  the  IDP,  on  the  contrary,  the  pose  of  the 
EE  is  given,  while  the  six  joint  variables  that  produce  this  pose  are  to  be  found. 

The  DDP  reduces  to  matrix  and  matrix-times-vector  multiplications;  as  we  shall 
show  presently,  the  DDP  poses  no  major  problem.  The  IDP,  however,  is  more 
challenging,  for  it  involves  intensive  variable-elimination  and  nonlinear-equation 
solving.  Indeed,  in  the  most  general  case,  the  IDP  amounts  to  eliminating  five  out 
of  the  six  unknowns,  with  the  aim  of  reducing  the  problem  to  a  single  mono  variate 
polynomial  of  16th  degree  or  lower.  While  finding  the  roots  of  a  polynomial  of 
this  degree  is  no  longer  an  insurmountable  task,  reducing  the  underlying  system 
of  nonlinear  equations  to  a  monovariate  polynomial  requires  intensive  computer- 
algebra  work  that  must  be  very  carefully  planned  to  avoid  the  introduction  of 
spurious  roots  and,  with  this,  an  increase  in  the  degree  of  that  polynomial.  Eor  this 
reason,  we  limit  this  chapter  to  the  study  of  the  geometric  IDP  of  decoupled  six-axis 
manipulators.  The  IDP  of  the  most  general  six-revolute  serial  manipulator  is  studied 
in  Chap.  9. 

In  studying  the  DDP  of  six-axis  manipulators,  we  need  not  limit  ourselves  to 
a  particular  architecture.  We  thus  study  here  the  DDP  of  manipulators  such  as  the 
one  sketched  in  Pig.  4.9.  This  manipulator  consists  of  seven  rigid  bodies,  or  links, 
coupled  by  six  revolute  joints.  Correspondingly,  we  have  seven  frames,  T\,  T2, 

. . .,  the  /th  frame  fixed  to  the  (/  —  l)st  link,  T\  being  termed  the  base  frame, 
because  it  is  fixed  to  the  base  of  the  manipulator.  Manipulators  with  joints  of  the 
prismatic  type  are  simpler  to  study  and  can  be  treated  using  correspondingly  simpler 
procedures. 

A  line  Ci  is  associated  with  the  axis  of  the  ith  revolute  joint,  and  a  positive 
direction  along  this  line  is  defined  arbitrarily  through  a  unit  vector  .  Eor  a  prismatic 
pair,  a  line  Ci  can  be  also  defined,  as  a  line  having  the  direction  of  the  pair  but 
whose  location  is  undefined;  the  analyst,  then,  has  the  freedom  to  locate  this  axis 
conveniently.  Thus,  a  rotation  of  the  /th  link  with  respect  to  the  (/  —  l)st  link  or 
correspondingly,  of  with  respect  to  Ti,  is  totally  defined  by  the  geometry  of 
the  /th  link,  i.e.,  by  the  DH  parameters  at,  bi,  and  at,  plus  and  its  associated 
joint  variable  Oi.  Then,  the  DH  parameters  and  the  joint  variables  define  uniquely 
the  posture  of  the  manipulator.  In  particular,  the  relative  position  and  orientation  of 
with  respect  to  is  given  by  matrix  Qf  and  vector  a^,  respectively,  which 
were  defined  in  Sect.  4.2  and  are  displayed  below  for  quick  reference: 


COS  Oi  —Xi  sin  Oi  fii  sin  Oi 

1 

0 

0 

_j 

Q,  = 

sin  Oi  Xi  cos  Oi  —jjii  cos  Oi 

,  a,  = 

a,-  sin  6i 

(4.7) 

_  0  jj^i  Xi  _ 

_  bi  _ 

Thus,  Qi  and  denote,  respectively,  the  matrix  rotating  into  an  orientation 
coincident  with  that  of  2^ +1  and  the  vector  joining  the  origin  of  Ci  with  that  of 
directed  from  the  former  to  the  latter.  Moreover,  Q/  and  a^ ,  as  given  in  Eq.  (4.7), 
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are  represented  in  Ti  coordinates.  The  equations  leading  to  the  geometric  model 
under  study  are  known  as  the  displacement  equations.  It  is  noteworthy  that  the 
problem  under  study  is  equivalent  to  the  input-output  analysis  problem  of  a  seven- 
revolute  linkage  with  one  degree  of  freedom  and  one  single  kinematic  loop  (Duffy 
1980).  Because  of  this  equivalence  with  a  closed  kinematic  chain,  sometimes  the 
displacement  equations  are  also  termed  closure  equations.  These  equations  relate 
the  orientation  of  the  EE,  as  produced  by  the  joint  coordinates,  with  the  prescribed 
orientation  Q  and  the  position  vector  p  of  the  operation  point  P  of  the  EE.  That 
is,  the  orientation  Q  of  the  EE  is  obtained  as  a  result  of  the  six  individual  rotations 
{  Q/  }  j  about  each  revolute  axis  through  an  angle  Of ,  in  a  sequential  order,  from  1  to 
6.  If,  for  example,  the  foregoing  relations  are  expressed  in  J^i,  then 

[Q6]i[Q5]i[Q4]i[Q3]i[Q2]i[Qi]i  =  [Q]i  (4.8a) 
[ai]i  +  [a2]i  +  [a3]i  +  [a4]i  +  [asji  +  [a6]i  =  [p]i  (4.8b) 

Notice  that  the  above  equations  require  that  all  vectors  and  matrices  involved  be 
expressed  in  the  same  coordinate  frame.  However,  we  derived  in  Sect.  4.2  general 
expressions  for  Q,  and  a,  in  Ti,  Eqs.  (4.1e)  and  (4.3b),  respectively.  It  is  hence 
convenient  to  represent  the  foregoing  relations  in  each  individual  frame,  which  can 
be  readily  done  by  means  of  similarity  transformations.  Indeed,  if  we  apply  the 
transformations  (4.5a  and  b)  to  each  of  [a,  ]i  and  [Q;  ]i,  respectively,  we  obtain  a, 
or,  correspondingly,  Q,  in  .  Therefore,  Eq.  (4.8a)  becomes 

[Qi]i[Q2]2[Q3]3[Q4]4[Q5]5[Q6]6  =  [Q]l 

Now  for  compactness,  let  us  represent  [Q]i  simply  by  Q  and  let  us  recall  the 
abbreviated  notation  introduced  in  Eq.  (4.1d),  where  [Qt  ]/  is  denoted  simply  by 
Q; ,  thereby  obtaining 


Q1Q2Q3Q4Q5Q6  —  Q 


(4.9a) 


Likewise,  Eq.  (4.8b)  becomes 

+  Ql(a2  +  02^3  +  0203^4  +  Q2Q3Q4a5  +  Q2Q3Q4Q5a6)  =  P  (4.9b) 

in  which  both  sides  are  given  in  base-frame  coordinates.  Equations  (4.9a  and  b) 
above  can  be  cast  in  a  more  compact  form  if  homogeneous  transformations,  as 
defined  in  Sect.  2.5,  are  now  introduced.  Thus,  if  we  let  Tf  =  {  Tf  }i  be  the  4x4 
matrix  transforming  2^ +i -coordinates  into  -coordinates,  the  foregoing  equations 
can  be  written  in  4  x  4  matrix  form,  namely. 


T1T2T3T4T5T6  =  T 


(4.10) 
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with  T  denoting  the  transformation  of  coordinates  from  the  end-effector  frame  to 
the  base  frame.  Thus,  T  contains  the  pose  of  the  end-effector. 

In  order  to  ease  the  discussion  ahead,  we  introduce  now  a  few  definitions.  A 
scalar,  vector,  or  matrix  expression  is  said  to  be  multilinear  in  a  set  of  vectors  {  V/ 
if  each  of  those  vectors  appears  only  linearly  in  the  same  expression.  This  does  not 
prevent  products  of  components  of  those  vectors  from  occurring,  as  long  as  each 
product  contains  only  one  component  of  the  same  vector.  Alternatively,  we  can  say 
that  the  expression  of  interest  is  multilinear  in  the  aforementioned  set  of  vectors 
if  and  only  if  the  partial  derivative  of  that  expression  with  respect  to  vector  v/  is 
independent  of  ,  for  /  =  1 , . . . ,  A .  For  example,  every  matrix  Q/  and  every  vector 
a/,  defined  in  Eqs.  (4.1e)  and  (4.3b),  respectively,  is  linear  in  vector  x/,  where  x/  is 
defined  as 


COS  Oi 

sin  Oi 


(4.11) 


Moreover,  the  product  Q1Q2Q3Q4Q5Q6  appearing  in  Eq.  (4.9a)  is  hexalinear,  or 
simply,  multilinear,  in  vectors  {x/  }^.  Likewise,  the  sum  appearing  in  Eq.  (4.9b) 
is  multilinear  in  the  same  set  of  vectors.  By  the  same  token,  a  scalar,  vector,  or 
matrix  expression  is  said  to  be  multiquadratic  in  the  same  set  of  vectors  if  those 
vectors  appear  at  most  quadratically  in  the  said  expression.  That  is,  the  expression 
of  interest  may  contain  products  of  the  components  of  all  those  vectors,  as  long  as 
those  products  contain,  in  turn,  a  maximum  of  two  components  of  the  same  vector, 
including  the  same  component  squared.  Qualifiers  like  multicubic,  multiquartic, 
etc.,  bear  similar  meanings. 

Further,  we  partition  matrix  Q/  rowwise  and  columnwise,  namely, 

=  [Pi  Qi  Ui]  (4-12) 

It  is  noteworthy  that  the  third  row  oj  of  Q,  is  independent  of  Of ,  a  fact  that  will  be 
found  useful  in  the  forthcoming  derivations.  Furthermore,  note  that  according  to  the 
DH  notation,  the  unit  vector  in  the  direction  of  the  ith  joint  axis  in  Fig.  4.9  has 
Ti  -components  given  by 


m 


I 


n 


o 


I 

T 

i 

T 


(4.13) 


Henceforth,  e  is  used  to  represent  a  three-dimensional  array  with  its  last  component 
equal  to  unity,  its  other  components  vanishing.  Thus,  we  have 


QiO,  =  Qfu,  =  e 


(4.14a) 
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or 

u,=Q,e,  o,=Qfe  (4.14b) 

That  is,  if  we  regard  e  in  the  first  of  the  foregoing  relations  as  [e^+i  ]/+i,  and  as 
[  0/  ]/  in  the  second  relation,  then,  from  the  coordinate  transformations  of  Eqs.  (4.4a 
and  b). 


U;  =  [e,+i]i,  and  o,-  =  [ei],+i 


(4.15) 


4.4  The  Inverse  Displacement  Analysis  of  Decoupled 
Manipulators 

Industrial  manipulators  are  frequently  supplied  with  a  special  architecture  that 
allows  a  decoupling  of  the  positioning  problem  from  the  orientation  problem.  In 
fact,  a  determinant  design  criterion  in  this  regard  has  been  that  the  manipulator 
lend  itself  to  a  closed-form  inverse  displacement  solution.  Although  the  class  of 
manipulators  with  this  feature  is  quite  broad,  we  will  focus  on  a  special  kind, 
the  most  frequently  encountered  in  commercial  manipulators,  that  we  have  termed 
decoupled.  Decoupled  manipulators  were  defined  in  Sect.  4.2  as  those  whose  last 
three  joints  have  intersecting  axes.  These  joints,  then,  constitute  the  wrist  of  the 
manipulator,  which  is  said  to  be  spherical,  because  when  the  point  of  intersection 
of  the  three  wrist  axes,  C,  is  kept  fixed,  all  the  points  of  the  wrist  move  on  spheres 
centered  at  C.  In  terms  of  the  DH  parameters  of  the  manipulator,  in  a  decoupled 
manipulator  =  as  =  bs  =  0,  and  thus,  the  origins  of  frames  5  and  6 
are  coincident.  All  other  DH  parameters  can  assume  arbitrary  values.  A  general 
decoupled  manipulator  is  shown  in  Fig.  4.10,  where  the  wrist  is  represented  as  a 
concatenation  of  three  revolutes  with  intersecting  axes. 

In  the  two  subsections  below,  a  procedure  is  derived  for  determining  all  the 
inverse  displacement  solutions  of  decoupled  manipulators.  In  view  of  the  decoupled 
architecture  of  these  manipulators,  we  conduct  their  displacement  analysis  by 
decoupling  the  positioning  problem  from  the  orientation  problem. 


4.4.1  The  Positioning  Problem 

We  solve  first  the  positioning  problem.  Let  C  denote  the  intersection  of  axes  4,  5, 
and  6,  i.e.,  the  center  of  the  spherical  wrist,  and  let  c  denote  the  position  vector  of 
this  point.  Apparently,  the  position  of  C  is  independent  of  joint  angles  O4,  O5,  and 
Os',  hence,  only  the  first  three  joints  are  to  be  considered  for  this  analysis.  The  arm 
structure  depicted  in  Fig.  4.1 1  will  then  be  analyzed.  From  that  figure. 


+  Qia2  +  QiQ2a3  +  QiQ2Q3a4  —  c 


(4.16) 
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Fig.  4.10  A  general  6R  manipulator  with  decoupled  architecture 


where  the  two  sides  are  expressed  in  -coordinates.  This  equation  can  be  readily 
rewritten  in  the  form 


32  +  0233  +  Q2Q334  =  Q[(c  -  ai) 


or  if  we  recall  Eq.  (4.3d), 

Q2(t>2  +  Q3t>3  +  Q3Q4l>4)  =  Qfc  —  bi 

However,  since  we  are  dealing  with  a  decoupled  manipulator,  we  have,  from 
Fig.  4.10, 


a4  =  Q4b4 


which  has  been  rewritten  as  the  product  of  constant  b/[  times  the  unit  vector  e  defined 
in  Eq.  (4.13). 

Thus,  the  product  Q3Q4b4  reduces  to 


Q3Q4b4  =  /?4Q3C  =  ^4113 


with  U;  defined  in  Eq.  (4.14b).  Hence,  Eq.  (4.16)  leads  to 


Q2(b2  +  Q3b3  +  M3)  =  Q[c  -  bi 


(4.17) 
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Further,  an  expression  for  c  can  be  derived  in  terms  of  p,  the  position  vector  of  the 
operation  point  of  the  EE,  and  Q,  namely, 

C  =  p  —  QlQ2Q3Q4a5  —  QlQ2Q3Q4Q5a6  (4.18a) 


Now,  since  as  =  bs  =  0,  we  have  that  as  =  0,  Eq.  (4.18a)  thus  yielding 

c  =  p-QQgae  =  p-Qbe  (4.18b) 


Moreover,  the  base  coordinates  of  P  and  C ,  and  hence,  the  J^i  -components  of  their 
position  vectors  p  and  c,  are  defined  as 


[p]l  = 

1 

1 _ 

,  [C]l  = 

~^c~ 

yc 

_Z_ 

_^c_ 

so  that  Eq.  (4.18b)  can  be  expanded  in  the  form 


~^c~ 

"x  -  (^11^6  +  qnhl^e  + 

yc 

— 

y  -  (^21^6  +  +  ^23^6^6) 

(4.18c) 

_^c_ 

_Z  -  (^3106  +  ^32^6/^6  +  ^33^6^6)  _ 

where  qij  is  the  {i ,  j  )  entry  of  [Q]  i ,  and  the  positioning  problem  now  becomes  one 
of  finding  the  first  three  joint  angles  necessary  to  position  point  C  at  a  point  of  base 
coordinates  xc ,  Jc  ^  and  zc  •  We  thus  have  three  unknowns,  but  we  also  have  three 
equations  at  our  disposal,  namely,  the  three  scalar  equations  of  Eq.  (4.17),  and  we 
should  be  able  to  solve  the  problem  at  hand. 

In  solving  the  foregoing  system  of  equations,  we  first  note  that  (a)  the  left-hand 
side  of  Eq.  (4.17)  appears  multiplied  by  Q2;  and  (b)  O2  does  not  appear  in  the  right- 
hand  side.  This  implies  that  (a)  if  the  Euclidean  norms  of  the  two  sides  of  that 
equation  are  equated,  the  resulting  equation  will  not  contain  62',  and  (b)  the  third 
scalar  equation  of  the  same  equation  is  independent  of  O2,  by  virtue  of  the  structure 
of  the  Q,  matrices  displayed  in  Eq.  (4.1e).  Thus,  we  have  two  equations  free  of  O2, 
which  allows  us  to  calculate  the  two  remaining  unknowns  0\  and  0^ . 

Let  the  Euclidean  norm  of  the  left-hand  side  of  Eq.  (4.17)  be  denoted  by  /,  that 
of  its  right-hand  side  by  r .  We  then  have 


=  a^  -\-  “1“  ^3  “1“  ^3  “1“  ^4  “1“  ^^^03^3  “1“  2Z?4b^ii3  “h  'l.Xsbsb^ 


from  which  it  is  apparent  that  is  linear  in  X3  and  is  linear  in  xi,  for  x/  defined 
in  Eq.  (4.11).  Upon  equating  with  r^,  then,  an  equation  linear  in  xi  and  X3 — not 
bilinear  in  these  vectors — is  readily  derived,  namely. 


Ac\  4“  Bs\  4“  Ccs  4“  E  —  0 


(4.19a) 
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Fig.  4.11  Three-axis,  serial,  positioning  manipulator 


whose  coefficients  do  not  contain  any  unknown,  i.e., 

A  =  2aiXc  (4.19b) 

B  =  2aiyc  (4.19c) 

C  =  2a2Ci3  —  2b2b4iji2iJi3  (4.19d) 

D  =  2a3b2lji2  +  2(32/?4/X3  (4.19e) 

E  =  al  +  al  +  bl  +  bj  +  bj  -  -  xj;  -  -  (zc  -  bif 

-\-2b2b3X2  2/72^42-22-3  “h  2/73/7423  (4.19f) 

Moreover,  the  third  scalar  equation  of  Eq.  (4.17)  takes  the  form 

F Cl  4“  Gsi  “h  Hc3  ^ S3  J  —  0  (4.20a) 

whose  coefficients,  again,  do  not  contain  any  unknown,  as  shown  below: 

F  =  ycl^i  (4.20b) 

G  =  — xc/xi  (4.20c) 
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H  =  — /?4/X2/X3 

(4.20d) 

I  =  (23/X2 

(4.20e) 

J  =  b2  -\-  ^32-2  +  ^4X2^3  —  {zc  ~  bi)Xi 

(4.20f) 

Thus,  we  have  derived  two  nonlinear  equations  in  0i  and  0^  that  are  linear  in  Ci, 
^1,  C3,  and  ^3.  Each  of  these  equations  thus  defines  a  contour  in  the  O1-O3  plane,  their 
intersections  determining  all  real  solutions  to  the  problem  at  hand. 

Now,  two  well-known  trigonometric  identities  are  introduced,  namely. 


C3  = 


1  —  r? 

l  +  r| 


^3  =  where  13  =  tan 


(4.21) 


Henceforth,  the  foregoing  identities  will  be  referred  to  as  the  tan-half-angle 
identities.  We  will  be  resorting  to  them  throughout  the  book.  Note  that  if  c,  and 
Si  are  substituted  for  their  equivalents  in  terms  of  tan(0//2),  for  /  =  1,3,  then  two 
biquadratic  polynomial  equations  in  tan(0i/2)  and  idcniO^/l)  are  derived.  Thus,  one 
can  eliminate  one  of  these  variables  from  the  foregoing  equations,  thereby  reducing 
the  two  equations  to  a  single  quartic  polynomial  equation  in  the  other  variable.  The 
quartic  equation  thus  resulting  is  called  the  characteristic  equation  of  the  problem  at 
hand.  Alternatively,  the  two  above  equations,  Eqs.  (4.19a)  and  (4.20a),  can  be  solved 
for,  say,  Ci  and  in  terms  of  the  data  and  C3  and  ^3,  namely. 


Cl  = 


Si  = 


—G(Cc3  +  Dss  E)  -h  B{Hc3  +  I  S3  +  J) 
F(CC3  +  DS3  E)  —  A(HC3  +  I  S3  +  J) 


(4.22a) 


(4.22b) 


with  Ai  defined  as 


Ai  =  AG  —  EB  =  —2aiiii{x^  +  j^) 


(4.22c) 


Note  that  in  trajectory  planning,  to  be  studied  in  Chap.  6,  Ai  can  be  computed 
off-line,  i.e.,  prior  to  setting  the  manipulator  into  operation,  for  it  is  a  function  solely 
of  the  manipulator  parameters  and  the  Cartesian  coordinates  of  a  point  lying  on 
the  path  to  be  tracked.  Moreover,  the  above  calculations  are  possible  as  long  as  Ai 
does  not  vanish.  Now,  Ai  vanishes  if  and  only  if  any  of  the  factors  ai,  /Xi,  and 
+  Tc  The  first  two  conditions  are  architecture-dependent,  whereas  the 

third  is  position-dependent.  The  former  occur  frequently  in  industrial  manipulators, 
although  not  both  at  the  same  time.  If  both  parameters  ai  and  /xi  vanished,  then  the 
arm  would  be  useless  to  position  arbitrarily  a  point  in  space.  The  third  condition, 
i.e.,  the  vanishing  of  +  Jc’  that  point  C  lies  on  the  Zi  axis.  Now,  even 

if  neither  ai  nor  /xi  vanishes,  the  manipulator  can  be  postured  in  a  configuration 
at  which  point  C  lies  on  the  Zi  axis.  Such  a  configuration  is  termed  the  first 
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singularity.  Note,  however,  that  with  point  C  being  located  on  the  Zi  axis,  any 
motion  of  the  first  joint,  with  the  two  other  joints  locked,  does  not  change  the 
location  of  C.  For  the  moment,  it  will  be  assumed  that  Ai  does  not  vanish,  the 
particular  cases  under  which  it  does  being  studied  presently.  Next,  both  sides  of 
Eqs.  (4.22a  and  b)  are  squared,  the  squares  thus  obtained  are  then  added,  and  the 
sum  is  equated  to  1 ,  which  leads  to  a  quadratic  equation  in  X3 ,  namely. 


Kc^  Nc^  P S'^  Q  —  0 

(4.23) 

whose  coefficients,  after  simplification,  are  given  below: 

K  = 

(4.24a) 

L  =  Aa\r  + 

(4.24b) 

M  =  2{Aa\HI  +  ix\CD) 

(4.24c) 

N  =  2{Aa\HJ  +  ii\CE) 

(4.24d) 

P  =  2{Aa\lJ  +  ix\DE) 

(4.24e) 

Q  =  Aa\j^  +  tx\E^  —  4a\iJ,\p^ 

(4.24f) 

with  defined  as 

2  2,2 

P  =  +  Tc 

Upon  substitution  of  the  tan-half  identities  introduced  in 
Eq.  (4.23),  a  quartic  equation  in  13  is  obtained,  i.e.. 

Eq.  (4.21)  into 

Rx^  +  Sxl  +  Txl  +  C/rs  +  F  =  0 

(4.25) 

whose  coefficients  are  all  computable  from  the  data.  After  some 
these  coefficients  take  on  the  forms 

simplifications. 

R  =  Aa\{J  —  +  ptliE  —  C)^  —  Ap^a\iJi\ 

(4.26a) 

S  =  A[Aa\l{J  -  77)  +  -  C)] 

(4.26b) 

T  =  2[Aa\{J^  -  +  2/2)  +  iJ,l{E^  -  +  2D^) 

—4p^a\pL\] 

(4.26c) 

U  =  4[4alliH  +  J)  +  ii\D{C  +  E)] 

(4.26d) 

V  =  4a\{J  +  Hf  +  ix]{E  +  Cf  -  4p^a\pi\ 

(4.26e) 

Furthermore,  let  {  (13)/  be  the  four  roots  of  Eq.  (4.25).  Thus,  up  to  four  possible 
values  of  O3  can  be  obtained,  namely, 

(63)1  =  2arctan[(r3),],  i  =  1,2,  3, 4  (4.27) 
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Once  the  four  values  of  O3  are  available,  each  of  these  is  substituted  into 
Eqs.  (4.22a  and  b),  which  thus  produce  four  different  values  of  Oi.  For  each  value  of 
0i  and  O3,  then,  one  value  of  O2  can  be  computed  from  the  first  two  scalar  equations 
of  Eq.  (4.17),  which  are  displayed  below: 

All  cos  O2  +  Ai2  sin  O2  =  xc  cos  0i  +  yc  sin  0i  —  ai 
—Ai2  cos  O2  +  All  sin  O2  =  —xc^i  sin  0i  +  yc^i  cos  0i 

+  (zc  ~  bi)fii 

where 

All  =  <^2  +  <33  cos  O3  +  Z?4/X3  sin  O3  (4.28c) 

v4i2  =  —a3X2  sin  O3  +  /?3/X2  +  b^X2lJi3  cos  O3  +  /?4/X2A3  (4.28d) 

Thus,  if  v4ii  and  y4i2  do  not  vanish  simultaneously,  angle  O2  is  readily  computed 
in  terms  of  0i  and  O3  from  Eqs.  (4.28a  and  b)  as 

cos  ^2  =  -^{  ^11  (-^c  cos  0i  +  yc  sin 0i  —  ai) 

—Ai2[—xcXi  sin  0i  +  cos  0i 
+  (zc  -^i)/xi]}  (4.29a) 

sin  02  =  ^i2(-^c  cos  01  +  yc  sin0i  -  ai) 

+  ^ii[— xcAi  sin  01  +  yc^i  cos  0i 
+  (zc-bi)/xi]}  (4.29b) 


(4.28a) 

(4.28b) 


where  A 2  is  defined  as 

A2  =  ^11  +  ^12 

=  ^2  +  al(cos^  03  +  A2  sin^  O3)  +  blixl(sin^  63  +  X\  cos^  63) 

+  2a2a3  cos  O3  +  2a2/?4/X3  sin  O3 
+  2A2/X2(^3  +  b/iX3){b^iJi3  cos  03  —  <23  sin  O3) 

-\-2a3b4ijiliji3  sin  03  cos  03  +  (b3  +  X3b4)^iJi\  (4.29c) 

the  case  in  which  A2  =  0,  which  leads  to  what  is  termed  here  the  second  singularity, 
being  discussed  presently. 

Takano  (1985)  considered  the  solution  of  the  positioning  problem  for  all 
possible  combinations  of  prismatic  and  revolute  pairs  in  the  regional  structure  of 
a  manipulator.  A  sketch  of  an  intermediate  P  joint  in  a  kinematic  chain  is  displayed 
in  Fig.  7.3.  Takano  found  that: 
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1.  In  the  case  of  arms  containing  either  three  revolutes,  or  two  revolutes  and  one 
prismatic  pair,  with  a  general  layout  in  all  cases,  a  quartic  equation  in  cos  O3  is 
obtained; 

2.  in  the  case  of  one  revolute  and  two  prismatic  pairs,  the  positioning  problem  was 
reduced  to  a  single  quadratic  equation,  the  problem  at  hand  thus  admitting  two 
solutions; 

3.  finally,  for  three  prismatic  pairs,  one  single  linear  equation  was  derived,  the 
problem  thus  admitting  a  unique  solution. 


The  Vanishing  of  A 1 

In  the  above  derivations  we  have  assumed  that  neither  /xi  nor  vanishes.  However, 
if  either  /Xi  =  0  or  =0,  then  one  can  readily  show  that  Eq.  (4.25)  reduces  to 
a  quadratic  equation,  and  hence,  this  case  differs  essentially  from  the  general  one. 
Note  that  one  of  these  conditions  can  occur,  and  the  second  occurs  indeed  frequently, 
but  both  together  never  occur,  because  their  simultaneous  occurrence  would  render 
the  axes  of  the  first  two  revolutes  coincident.  The  manipulator  would  thus  be  short 
of  one  joint  for  the  execution  of  three-dimensional  tasks.  We  thus  have  two  cases: 

1.  /xi  =  0,  7^  0.  In  this  case, 

A,  B  ^0,  F  =  G  =  0 

Under  these  conditions,  Eq.  (4.20a)  and  the  tan-half-angle  identities  given  in 
Eq.  (4.21)  yield 


(J  -  H)rl  +  2/T3  +  {J  +  H)  =  0 


which  thus  produces  two  values  of  13 ,  namely. 


(U)l,2  — 


-I  zb  V/2 

J -H 


(4.30a) 


Once  two  values  of  0^  have  been  determined  according  to  the  above  equation,  0i 
can  be  found  using  Eq.  (4.19a)  and  the  tan-half- angle  identities,  thereby  deriving 


{E'  -  A)xl  +  IBxi  F{E'  F  A)  =  ^ 


where 


Q 

E'  =  Cc3  Ds3  E,  Xi  =  tan  (  ^ 
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whose  roots  are 


(g)i,2 


-B  zb  _  E'l  ^2 


(4.30b) 


Thus,  two  values  of  0i  are  found  for  each  of  the  two  values  of  ^3,  which  results 
in  four  positioning  solutions.  Values  of  O2  are  obtained  using  Eqs.  (4.29a  and  b). 
2.  =  0,  /xi  7^  0.  In  this  case,  one  has  an  architecture  similar  to  that  of  the  robot 

of  Fig.  4.3.  We  have  now 


A  =  B  =  0,  F,  Gt^O 
Under  the  present  conditions,  Eq.  (4.19a)  reduces  to 

{E  -  C)xl  +  2Dt3  +  (E  +  C)  =  0 


which  produces  two  values  of  13 ,  namely. 


(U)l,2  — 


-D  ± 

E-C 


(4.31a) 


With  the  two  values  of  0^  obtained,  0i  can  be  found  using  Eq.  (4.20a)  and  the 
tan-half-angle  identities  to  produce 


(/'  -  E)xl  +  2Gri  +  (/'  +  F)  =  0 


where 


J  —  ^ J,  Xi 


tan 


whose  roots  are 


(U)l,2 


-G  zb  VG2  -  f2 

J'  -E 


(4.31b) 


Once  again,  the  solution  results  in  a  cascade  of  two  quadratic  equations,  one 
for  O3  and  one  for  Oi,  which  yields  four  positioning  solutions.  As  above,  O2  is 
then  determined  using  Eqs.  (4.29a  and  b).  Note  that  for  the  special  case  of  the 
manipulator  of  Fig.  4.3,  we  have 


ai  =  b2  =  0,  ai  =  as  =  90°,  a2  =  0° 
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Fig.  4.12  The  four  arm 
configurations  for  the 
positioning  problem  of  the 
Puma  robot:  (a,  b)  elbow 
down;  (a,  c)  shoulder  fore;  (c, 
d)  elbow  up;  (b,  d)  shoulder 
aft 


a 


and  hence, 

//  =  /  =  0,  E  =  -\-  CL^  -\-  b 2  +  y'^  +  (zc  ~  ^i)^]  ? 

C  =  2a2a3,  D  =  2a2b/[,  F  =  yc,  G  =  —xc,  J  =  b^ 


In  this  case,  the  foregoing  solutions  reduce  to 


fe)l,2 


-D  ±  +  D^-E^ 


E-C 


(■^^1)1,2 


Xc  ± 


y/^l  +  yl-H 
h  -  yc 


A  robot  with  the  architecture  studied  here  is  the  Puma,  which  is  displayed  in 
Fig.  4.12  in  its  four  distinct  postures  for  the  same  location  of  its  wrist  center.  Notice 
that  the  orientation  of  the  EE  is  kept  constant  in  all  four  postures. 
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The  Vanishing  of  A  2 

In  some  instances,  A2,  as  defined  in  Eq.  (4.29c),  may  vanish  at  a  certain  posture, 
thereby  preventing  the  calculation  of  O2  from  Eqs.  (4.29a  and  b).  This  posture, 
termed  the  second  singularity,  occurs  if  both  coefficients  An  and  Au  of  Eqs.  (4.28a 
and  b)  vanish.  Note  that  from  their  definitions,  Eqs.  (4.28c  and  d),  these  coefficients 
are  not  only  position-  but  also  architecture-dependent.  Thus,  an  arbitrary  manip¬ 
ulator  cannot  take  on  this  configuration  unless  its  geometric  dimensions  allow  it. 
This  type  of  singularity  will  be  termed  architecture-dependent,  to  distinguish  it  from 
others  that  are  common  to  all  robots,  regardless  of  their  particular  architectures. 

We  can  now  give  a  geometric  interpretation  of  the  singularity  at  hand:  Eirst,  note 
that  the  right-hand  side  of  Eq.  (4.17),  from  which  Eqs.  (4.28a  and  b)  were  derived, 
is  identical  to  (c  —  ai),  which  means  that  this  expression  is  nothing  but  the 
7^2 -representation  of  the  position  vector  of  C.  That  is,  the  components  of  vector 

Q|  (c  —  ai)  are  the  7^2 -components  of  vector  O2C .  Therefore,  the  right-hand  sides 

- > 

of  Eqs.  (4.28a  and  b)  are,  respectively,  the  X2-  and  T2-components  of  vector  O2C . 
Consequently,  if  =  A12  =  0,  then  the  two  foregoing  components  vanish  and, 
hence,  point  C  lies  on  the  Z2  axis.  The  first  singularity  thus  occurs  when  point  C 
lies  on  the  axis  of  the  first  revolute,  while  the  second  occurs  when  the  same  point 
lies  on  the  axis  of  the  second  revolute. 

Many  industrial  manipulators  are  designed  with  an  orthogonal  architecture, 
which  means  that  the  angles  between  neighbor  axes  are  multiples  of  90° .  Moreover, 
with  the  purpose  of  maximizing  their  workspace,  orthogonal  manipulators  are 
designed  with  their  second  and  third  links  of  equal  lengths,  thereby  rendering  them 
vulnerable  to  this  type  of  singularity.  An  architecture  common  to  many  manipulators 
such  as  the  Cincinnati-Milacron,  ABB,  Eanuc,  and  others,  comprises  a  planar  two- 
axis  layout  with  equal  link  lengths,  which  is  capable  of  turning  about  an  axis 
orthogonal  to  these  two  axes.  This  layout  allows  for  the  architecture  singularity 
under  discussion,  as  shown  in  Eig.  4.13a.  The  well-known  Puma  robot  is  similar 
to  the  foregoing  manipulators,  except  that  it  is  supplied  with  what  is  called  a 
shoulder  offset  as  illustrated  in  Eig.  4.3.  This  offset,  however,  does  not  prevent 
the  Puma  from  attaining  the  same  singularity,  as  depicted  in  Pig.  4.13b.  Notice  that 
in  the  presence  of  this  singularity,  angle  O2  is  undetermined,  but  0i  and  0^  are 
determined  in  the  case  of  the  Puma  robot.  However,  in  the  presence  of  the  singularity 
of  Pig.  4.13a,  neither  0i  nor  O2  are  determined;  only  0^  of  the  arm  structure  is 
determined. 

Example  4.4.1.  A  manipulator  with  a  common  orthogonal  architecture  is  displayed 
in  Pig.  4. 14  in  an  arbitrary  configuration.  The  arm  architecture  of  this  manipulator 
has  the  DH  parameters  shown  below: 

ai  =  =  0,  bi  =  b2  =  =  0,  ai  =  90°,  0^2  =  0°,  as  =  90° 


Pind  its  inverse  kinematics  solutions. 
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Fig.  4.13  Architecture-dependent  singularities  of  (a)  the  Cincinnati-Milacron  and  (b)  the  Puma 
robots 


P(x,y,z) 


C(xc,yc,zc) 


Fig.  4.14  An  orthogonal  decoupled  manipulator 


Solution:  A  common  feature  of  this  architecture  is  that  it  comprises  a2  =  b/[.  In 
the  present  discussion,  however,  the  latter  feature  need  not  be  included,  and  hence, 
the  result  that  follows  applies  even  in  its  absence.  In  this  case,  coefficients  C ,  D, 
and  E  take  on  the  forms 


C  =  0,  D  =  2a2b4, 


2 


Hence, 


C  =  H  =  I  =  J  =  0 
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and  so 


=  0,  F  =  yc,  G  =  —xc 
The  radical  of  Eq.  (4.31b)  reduces  to  +  Tc  •  Thus, 


fdA  xc  ±  y/xl+yl  -1  ±  yi  +  {yc/xcr 

tan  —  =  - - -  =  - ; - 

\2  J  -yc  yc/^c 

Now  we  recall  the  relation  between  tan(0i/2)  and  tan  Oi,  namely, 

{ 0i\  —1  zb  J\  -\-  tan^  0i 

tan  —  =  - — - - 

V  2  y  tan  0i 

Upon  comparison  of  Eqs.  (4.32a)  and  (4.32b),  it  is  apparent  that 


=  arctan 


ZU 


(4.32a) 


(4.32b) 


a  result  that  can  be  derived  geometrically  for  this  simple  arm  architecture.  Given  that 
the  arctan(-)  function  is  double-valued,  its  two  values  differing  in  180°,  we  obtain 
here,  again,  two  values  for  0i .  On  the  other  hand,  0^  is  calculated  from  Eq.  (4.3  la)  as 


(U)l,2  — 


2 


thereby  obtaining  two  values  of  ^3.  As  a  consequence,  the  inverse  positioning 
problem  of  this  arm  architecture  admits  four  solutions  as  well.  These  solutions  give 
rise  to  two  pairs  of  arm  postures  that  are  usually  referred  to  as  elbow -up  and  elbow - 
down. 

Example  4.4.2.  Eind  all  real  inverse  displacement  solutions  of  the  manipulator 
shown  in  Eig.  4.15,  when  point  C  of  its  end-effector  has  the  base  coordinates 
C(0,  2a,  —a). 

Solution:  The  Denavit-Hartenberg  parameters  of  this  manipulator  are  derived 
from  Eig.  4.16,  where  the  coordinate  frames  involved  are  indicated.  In  defining  the 
coordinate  frames  of  that  figure,  the  Denavit-Hartenberg  notation  was  followed, 
with  Z4  defined,  arbitrarily,  as  parallel  to  Z3.  Erom  Eig.  4.16,  then,  we  have 


ai  =  a2  =  a^  =  b2  =  bs  =  a,  bi  =  b/[  =  0,  ai  =  a2  =  90°,  =  0° 


One  inverse  displacement  solution  can  be  readily  inferred  from  the  geometry  of 
Eig.  4.16.  Eor  illustration  purposes,  and  in  order  to  find  all  other  inverse  kinematic 
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Fig.  4.15  An  orthogonal  RRR  manipulator 


Fig.  4.16  The  coordinate  frames  of  the  orthogonal  RRR  manipulator 
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solutions,  we  will  use  the  procedure  derived  above.  To  this  end,  we  first  proceed 
to  calculate  the  coefficients  of  the  quartic  polynomial  equation,  Eq.  (4.25),  which 
are  given,  nevertheless,  in  terms  of  coefficients  K,  Q  of  Eqs.  (4.24a-f).  These 


168 


4  Geometry  of  Decoupled  Serial  Robots 


coefficients  are  given,  in  turn,  in  terms  of  coefficients  y4,  . . /  of  Eqs.  (4.19b-f) 
and  (4.20b-f).  We  then  proceed  to  calculate  all  the  necessary  coefficients  in  the 
proper  order: 

A  =  0,  B  =  4a^,  C  =  D  =  —E  =  2a^ 

F  =  2a,  G  =  H  =  0,  I  =  J  =  a 

Moreover, 

K  =  L  =  M  =  N  =  -8a^  P  =  0,  Q  =  -8a\ 

The  set  of  coefficients  sought  thus  reduces  to 

R  =  K-N  +  Q=Aa'^ 

S  =2{P  -M)  =  -16a 
T  =  2{Q  A- 2L-K)  = 

U  =  2{M  +  P)  =  \6a‘^ 

V  =  K  +  N  +  Q  =  -12a^ 


which  leads  to  a  quartic  equation,  namely, 

~\~  2x^  -\~  4t3  —  3  =  0 


with  four  real  roots: 


fe)!  —  ('^3)2  —  1,  ('^3)3  —  —1.  ('^3)4  —  3 

These  roots  yield  the  0^  values  that  follow: 

(^3)1  =  (^3)2  =  90°,  (03)3  =  -90°,  (03)4  =  143.13° 

The  quartic  polynomial  thus  admits  one  double  root,  which  means  that  at  the 
configurations  resulting  from  this  root,  two  solutions  meet,  thereby  producing  a 
singularity,  an  issue  that  is  discussed  in  Sect.  5.4.  Below,  we  calculate  the  remaining 
angles  for  each  solution:  Angle  0i  is  computed  from  relations  (4.22a-c),  where 
Ai  =  —8(2^. 

The  first  two  roots,  (03)1  =  (03)2  =  90°,  yield  C3  =  0  and  ^3  =  1.  Hence, 
Eqs.  (4.22a  and  b)  lead  to 


Cl  = 


Si  = 


+  J)  Aa^{a  +  a) 


Ai 

F(D  +  E) 


=  -1 


—Sa^ 

2a(2a^  —  2a^) 
—Sa^ 


=  0 
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Fig.  4.17  Manipulator  configuration  for  C(0,  a,  0) 


and  hence, 


With  6i  known,  62  is  computed  from  the  first  two  of  Eqs.  (4.17),  namely. 

Cl  =  0,  ^2  =  -1 


and  hence, 


O 


The  remaining  roots  are  treated  likewise,  thereby  obtaining 

(^1)3  =  90°,  {62)2  =  0,  (^1)4  =  143.13°,  (^2)4  =  0 

It  is  noteworthy  that  the  architecture  of  this  manipulator  does  not  allow  for  the 
second  singularity,  associated  with  A2  =  0. 

Example  4.4.3.  For  the  same  manipulator  of  Example  4.4.2,  find  all  real  inverse 
displacement  solutions  when  point  C  of  its  end-effector  has  the  base  coordinates 
C(0,  a,  0),  as  displayed  in  Fig.  4.17. 
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Fig.  4.18  Contours 
producing  the  two  real 
solutions  for  Example  4.4.3 


Solution:  In  this  case,  one  obtains,  successively, 

^  =  0,  B  =  C  =  D  =  E=  2a^, 

F  =  a,  G  =  0  H  =  0,  I  =  J  =  a 

K  =  L  =  M  =  N  =  P  =  I6a\  Q  = 

R  =  0,  S  =  16a"^,  T  =  32^"^,  U  =  48a\  V  =  16a^ 

Moreover,  for  this  case,  the  quartic  Eq.  (4.23)  degenerates  into  a  cubic  equation, 
namely. 


2t^  “h  3T3  “h  1  —  0 

whose  roots  are  readily  found  as 

(r3)i  =  -0.43016,  (13)2,3  =  -0.78492  zb  71.30714 

where  j  is  the  imaginary  unit,  i.e.,  j  =  That  is,  only  one  real  solution 

is  obtained,  namely,  (^3)1  =  —46.551°.  However,  shown  in  Fig.  4.17  is  a  quite 
symmetric  posture  of  this  manipulator  at  the  given  position  of  point  C  of  its  end- 
effector,  which  does  not  correspond  to  the  real  solution  obtained  above.  In  fact, 
the  solution  yielding  the  posture  of  Fig.  4.17  disappeared  because  of  the  use  of  the 
quartic  polynomial  equation  in  tan(03  / 2).  Note  that  if  the  two  contours  derived  from 
Eqs.  (4.19a)  and  (4.20a)  are  plotted,  as  in  Fig.  4.18,  their  intersections  yield  the  two 
real  roots,  including  the  one  leading  to  the  posture  of  Fig.  4.17. 
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The  explanation  of  how  the  fourth  root  of  the  quartic  equation  disappeared  is 
given  below:  Let  us  write  the  quartic  polynomial  in  full,  with  a  “small”  leading 
coefficient  €,  namely, 


6  ^^3  H”  3T3  “h  1  —  0 


Upon  dividing  both  sides  of  the  foregoing  equation  by  ,  we  obtain 


12  3  1 

€  -\  ^  2^  3^  4 

r3 


=  0 


Apparently,  the  original  equation  is  satisfied  as  €  ^  0  if  and  only  if  zboo, 

i.e,  if  O3  =  180°.  The  missing  root  is,  hence,  (^3)4  =  n.  The  remaining  angles  are 
readily  calculated  as 


(9i)i  =  -105.9°,  (^2)1  =  -149.35°,  (^1)4  =  180°,  (62)4  =  -90° 


4.4.2  The  Orientation  Problem 

Now  the  orientation  inverse  displacement  problem  is  first  formulated,  then  solved. 
This  problem  consists  in  determining  the  wrist  angles  that  will  produce  a  prescribed 
orientation  of  the  end-effector.  The  orientation,  in  turn,  is  given  in  terms  of  the 
rotation  matrix  Q  taking  the  end-effector  from  its  home  attitude  to  its  current  one. 
Alternatively,  the  orientation  can  be  given  by  the  natural  invariants  of  the  rotation 
matrix,  vector  e  and  angle  0.  In  any  event,  all  nine  components  of  matrix  Q  are 
known  in  Ti.  It  is  convenient  to  assume  a  columnwise  partitioning  of  [Q]i  similar 
to  that  of  Q/(=  [  Q;  ]; )  displayed  in  Eq.  (4.12),  namely, 

[Q]i  =  [pqu]  (4-33) 

Without  loss  of  generality  it  can  be  assumed  that  Z7  is  defined  parallel  to  — as 
the  chain  is  open,  the  analyst  is  free  to  define  Z7  at  will.  From  Definition  2.2.1,  then 
[u]\  =  [el]\  =  [e6]\,  and  05  included  in  Fig. 4.19.  Moreover,  since  Oi,  O2,  and  O3 
are  available,  Qi,Q2,  and  Q3  become  data  for  this  problem.  One  now  has  the  general 
layout  of  Fig.  4.19,  where  angles  {  Of  are  to  be  determined  from  the  problem  data, 
which  are  in  this  case  the  orientation  of  the  end-effector  and  the  architecture  of  the 
wrist;  the  latter  is  defined  by  angles  a 4  and  as,  neither  of  which  can  be  either  0  or  tt. 

Now,  since  the  orientation  of  the  end-effector  is  given,  the  components  of  [  05  ]  1 
are  known,  but  they  will  be  needed  in  frame  4.  A  coordinate  transformation  from 
frame  1  to  frame  4  can  be  readily  implemented  by  resorting  to  the  transformation 
given  in  Eq.  (4.6a): 


[^6)4  —  (QlQ2Q3)^[e6  ]l  =  (QiQ2Q3)^[u]i 


(4.34) 
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Fig.  4.19  General  architecture  of  a  spherical  wrist 


Let  the  components  of  [  05  ]4,  all  of  them  known,  be  defined  as 


(4.35) 


Moreover,  based  on  the  first  of  Eqs.  (4.15),  the  components  of  vector  05  in  are 
nothing  but  the  entries  of  the  third  column  of  matrix  Q4,  i.e.. 


/X4  sin  04 
— /X4  cos  04 
A4 


Furthermore,  vectors  05  and  05  make  an  angle  q^s,  and  hence, 

0^05  =  A5  or  [06  ]!  [^5  ]4  =  ^5 
upon  substitution  of  Eqs.  (4.35)  and  (4.36)  into  Eq.  (4.37),  we  obtain 


(4.36) 


(4.37) 


^/X4  sin  04  —  ?7/X4  cos  04  +  ^X4  =  Xs 


(4.38) 
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which  can  be  readily  transformed,  with  the  aid  of  the  tan-half-angle  identities,  into 
a  quadratic  equation  in  14  =  tan(04/2),  namely, 

(A5  -  r][i4  -  ^24)14  -  2^114X4  +  (A5  +  r]ii4  -  ^24)  =  0  (4.39) 

its  two  roots  being  given  by 


±  +  ^2)^2  _ 

Ta  =  - - TT -  (4.40) 

A5  —  ^A4  —  rifl4 

Note  that  the  two  foregoing  roots  are  real  as  long  as  the  radical  is  positive,  the  two 
roots  merging  into  a  single  one  when  the  radical  vanishes.  Thus,  a  negative  radical 
means  an  attitude  of  the  EE  that  is  not  feasible  with  the  wrist.  It  is  noteworthy 
that  a  three-revolute  spherical  wrist  is  kinematically  equivalent  to  a  spherical  joint. 
However,  the  spherical  wrist  differs  essentially  from  a  spherical  joint  in  that  the 
latter  has,  kinematically,  an  unlimited  workspace — a  physical  spherical  joint,  of 
course,  has  a  limited  workspace  by  virtue  of  its  mechanical  construction — and 
can  orient  a  rigid  body  arbitrarily.  Therefore,  the  workspace  W  of  the  wrist  is  not 
unlimited,  but  rather  defined  by  the  set  of  values  of  rj,  and  ^  that  satisfy  the  two 
relations  shown  below: 


=  1  (4.41a) 

/(f ,  7],  +  r]^)/xl  -  (X5  -  ^Xa)^  >  0  (4.41b) 

In  view  of  condition  (4.41a),  however,  relation  (4.41b)  simplifies  to  an  inequality  in 
^  alone,  namely. 


FiO  ^  -  XXaXs^  -  (Ai"  -  a2)  <  0  (4.42) 


As  a  consequence, 

1 .  W  is  a  region  of  the  unit  sphere  S  centered  at  the  origin  of  the  three-dimensional 
space; 

2.  W  is  bounded  by  the  two  parallels  given  by  the  roots  of  F(^)  =  0  on  the  sphere; 

3.  the  wrist  attains  its  singular  configurations  along  the  two  foregoing  parallels. 

In  order  to  gain  more  insight  on  the  shape  of  the  workspace  W,  let  us  look  at  the 
boundary  defined  by  F(J^)  =  0.  Upon  setting  F{^)  to  zero,  we  obtain  a  quadratic 
equation  in  whose  two  roots  can  be  readily  found  to  be 

^1,2  =  A4A5  zb  I/X4/X5I  (4.43) 

which  thus  defines  two  planes,  /7i  and  772,  parallel  to  the  ^-rj  plane  of  the  three- 
dimensional  space,  intersecting  the  ^-axis  at  and  ^2,  respectively.  Thus,  the 
workspace  W  of  the  spherical  wrist  at  hand  is  that  region  of  the  surface  of  the  unit 
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Sphere  S  contained  between  the  two  parallels  defined  by  TJi  and  772.  For  example,  a 
common  wrist  design  involves  an  orthogonal  architecture,  i.e.,  0:4  =  0^5  =  90°.  For 
such  wrists. 


^1,2  =  ±1 

and  hence,  orthogonal  wrists  become  singular  when  [e6]4  =  [0,  0,  ±1]^,  i.e., 
when  the  fourth  and  the  sixth  axes  are  aligned.  Thus,  the  workspace  of  orthogonal 
spherical  wrists  is  the  whole  surface  of  the  unit  sphere  centered  at  the  origin,  the 
singularity  curve  thus  degenerating  into  two  points,  namely,  the  two  intersections  of 
this  sphere  with  the  ^-axis.  If  one  views  ^  =  0  as  the  equatorial  plane,  then  the  two 
singularity  points  of  the  workspace  are  the  poles. 

An  alternative  design  is  the  so-called  three-roll  wrist  of  some  Cincinnati- 
Milacron  robots,  with  a/[  =  =  120°,  thereby  leading  to  A4  =  A5  =  —1/2 

and  iiA  =  1^5  =  V3/2.  For  this  wrist,  the  two  planes  TJi  and  772  are  found  below: 
First,  we  note  that  with  the  foregoing  architecture. 


and  hence,  the  workspace  of  this  wrist  is  the  part  of  the  surface  of  the  unit  sphere 
S  that  lies  between  the  planes  77i  and  772  parallel  to  the  ^-rj  plane,  intersecting  the 
^-axis  at  =  1  and  ^2  =  —1/2,  respectively.  Hence,  if  ^  =  0  is  regarded  as  the 
equatorial  plane,  then  the  points  of  the  sphere  S  that  are  outside  of  the  workspace 
of  this  wrist  are  those  lying  at  a  latitude  smaller  than  —30°.  The  singularity  points 
are  thus  the  north  pole  and  those  lying  on  the  parallel  of  latitude  —30°. 

Once  O4  is  calculated  from  the  two  foregoing  values  of  14,  if  these  are  real,  angle 
O5  is  obtained  uniquely  for  each  value  of  O4,  as  explained  below:  First,  Eq.  (4.9a) 
is  rewritten  in  a  form  in  which  the  data  are  collected  in  the  right-hand  side,  which 
produces 


Q4Q5Q6  =  R  (4.44a) 

with  R  defined  as 

R  =  Q3^Q[q[Q  (4.44b) 

Moreover,  let  the  entries  of  R  in  the  fourth  coordinate  frame  be  given  as 


Gi  ru  ri3 
^21  ^22  ^23 
_^31  ^32  ^33_ 


Expressions  for  O5  and  0^  can  be  readily  derived  by  solving  first  for  Q5  from 
Eq.  (4.44a),  namely. 


Qs  =  Qlml 


(4.45) 
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Now,  by  virtue  of  the  form  of  the  Qi  matrices,  as  appearing  in  Eq.  (4.1e),  it  is 
apparent  that  the  third  row  of  Qi  does  not  contain  Ot .  Hence,  the  third  column  of 
the  matrix  product  of  Eq.  (4.45)  is  independent  of  0^.  Thus,  two  equations  for  O5  are 
obtained  by  equating  the  first  two  components  of  the  third  columns  of  that  equation, 
thereby  obtaining 

H5S5  =  {lX(,ri2  +  A6ri3)C4  +  (/^6?’22  +  Kr2'i)SA 
-jisCs  =  -XA{lX(,rn  +  Xeri3)s4  +  XA{l^6r22  +  Xer23)cA  +  /W,4(/W.6?'32  +  X^r^i) 

which  thus  yield  a  unique  value  of  O5  for  every  value  of  O4.  Einally,  with  O4  and  O5 
known,  it  is  a  simple  matter  to  calculate  0^.  This  is  done  upon  solving  for  Q6  from 
Eq.  (4.44a),  i.e.. 


Q6  =  Qs^QlR 


and  if  the  partitioning  (4.12)  of  Qi  is  now  recalled,  a  useful  vector  equation  is 
derived,  namely. 


P6  =  Q5  Q4  ri 


(4.46) 


where  ri  is  the  first  column  of  R.  Let  w  denote  the  product  Q4  ri,  i.e., 


w  =  Q[ri 


rnCA  +  r2iSA 

-A4(rii54  -  r2iC4)  +  iiAr^i 

llAimSA  -  ''21C4)  +  A4r3i 


Hence, 


W1C5  +  W2^5 

A5(— Wi^5  +  W2C5)  +  W3/X5 
,  —  ^2(^5)  +  ^3^5 


in  which  w/  denotes  the  i  th  component  of  w.  Hence,  and  are  determined  from 
the  first  two  scalar  equations  of  Eq.  (4.46),  namely. 


Ce  =  W1C5  +  W2^5 


thereby  deriving  a  unique  value  of  0^  for  every  pair  of  values  (O4,  O5).  In  summary, 
then,  two  values  of  O4  have  been  determined,  each  value  determining,  in  turn, 
one  single  corresponding  set  of  O5  and  0^  values.  Therefore,  there  are  two  sets  of 
solutions  for  the  orientation  problem  under  study,  which  lead  to  two  corresponding 
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a 


Fig.  4.20  The  two  configurations  of  a  three-axis  spherical  wrist 


wrist  postures.  The  two  distinct  postures  of  an  orthogonal  three-revolute  spherical 
wrist  for  a  given  orientation  of  its  EE  are  displayed  in  Eig.  4.20. 

When  combined  with  the  four  postures  of  a  decoupled  manipulator  leading  to 
one  and  the  same  location  of  its  wrist  center — positioning  problem — a  maximum  of 
eight  possible  combinations  of  joint  angles  for  a  single  pose  of  the  end-effector  of  a 
decoupled  manipulator  are  found. 

Example  4.4.4.  A  three-roll  wrist  is  mounted  on  the  orthogonal  manipulator  of 
Eig.  4. 17,  as  this  finds  itself  at  the  posture  shown  in  the  same  figure,  so  that  the  center 
of  the  wrist  coincides  with  point  C  of  the  orthogonal  manipulator.  The  assembly 
is  depicted  in  Eig.  4.21,  which  shows  axis  Z4  parallel  to  Zi.  Moreover,  the  EE  is 
desired  to  attain  the  orientation  given  by  matrix  Q,  defined  below  in  -coordinates: 


-1/3  -2/3  2/3 
-2/3  -1/3  -2/3 
2/3  -2/3  -1/3 


Eind  the  inverse-displacement  solutions  of  the  wrist  that  corresponds  to  the  fourth 
solution  found  in  Example  4.4.3  for  the  first  three  joint  angles.  Moreover,  these 
angles  can  also  be  found  from  Eig.  4.21  by  inspection. 

Eurthermore,  to  complete  the  DH  parameters,  Z7  is  defined  as  passing  through 
C  and  P ,  in  the  directions  from  the  former  to  the  latter,  which  is  assumed  to  yield 
an  angle  =  0.  The  three-roll  wrist  is  illustrated  in  Eig.  4.22. 

Solution:  As  the  reader  can  readily  verify,  the  arm  inverse-displacement  solution 
displayed  in  Eig.  4.21  is 


0l  =  7t,  O2 


O3  =  7t 
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Fig.  4.21  The  coordinate  frames  of  the  orthogonal  RRRR  manipulator 


64 


Fig.  4.22  A  representation  of  the  kinematic  chain  of  the  Cincinnati-Milacron  three-roll  wrist 
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Further,  given  the  wrist  architecture, 


^4  —  — 


V3 

2 


and,  in  light  of  the  value  of  given  above. 


1?  /^6  ^ 

Now  we  need  05  and  05  in  T4;  we  have  [  05  ]4  displayed  in  Eq.  (4.36),  and  hence, 
[^5] 4  —  [  V^s94,  —V3c04,  —  1  ]^/2.  Moreover,  [05  ]i  is  simply  the  third  column  of 
Q,  namely,  [05]!  =  [2/3,  —2/3,  —1/3]^.  In  order  to  bring  the  components  of  05 
into  2^4,  a  coordinate  transformation  is  needed.  This  transformation  is  implemented 
by  means  of  the  transpose  of  the  rotation  matrix  Q123  =  Q1Q2Q3  that  carries 
into  2^4.  This  matrix  is  most  simply  found  from  Definition  2.2.1  as  applied  to  the 
foregoing  frames,  and  upon  inspection  of  Fig.  4.21.  Therefore, 


Qi23  —  Q1Q2Q3 


0-10 
1  0  0 

0  0  1 


Hence, 


—  QiQ2Qn^6]i  — 

0  1  0 

-10  0 

■  2/3  ■ 
-2f3 

— 

■  2/3  ■ 
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1 

0 

0 

_ 1 

.-1/3. 

.-1/3. 

which  thus  leads  to  the  equation  for  O4: 

2  +  Vs  sin  O4  —  Vs  cos  ^4  =  0 

Upon  application  of  the  tan-half  identities  to  the  above  equation,  a  quadratic 
equation  in  14  =  tan(04/2)  is  derived,  namely, 

2  4-2  T4^  “h  2  VSx4  4“  Vs  VSx^  =  0 

whose  two  roots  lead  to  the  two  values  of  ^4  given  below: 

6>4  1  =  -9.7356103370°,  6^4,2  =  80.2643896700° 
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Further,  matrix  R  of  Eq.  (4.44b)  is  obtained  as 


2/3  1/3  2/3 

-1/3  -2/3  2/3 
2/3  -2/3  -1/3 


Upon  substitution  of  the  above  expression  into  Eq.  (4.45),  expressions  for  cs  and  ^5 
are  obtained  in  terms  of  O4: 


C5 


V3 

9 


^—2  sin  04  +  2  cos  O4  + 


and 


^5 


4x/3 


(cos  04  +  sin  04) 


If  now  each  of  the  two  values  obtained  above  for  04  is  substituted  in  the  above 
equation,  the  corresponding  values  for  O5  are  obtained  as 

05  1  =  38.9424412300°,  65,2  =  -38.9424412600° 

Finally,  the  product  Q5Q4R  is  computed,  then  equated  to  Q6.  The  (1,  1)  and 
(2,  1)  entries  of  the  foregoing  product  yield  cos  06  and  sin  06,  respectively,  as  per 
Eq.  (4.46).  Two  values  are  thus  obtained  for  06,  one  for  each  pair  of  (04,  Os)  values: 

06  1  =  -39.4804916600°,  06,2  =  -110.0092709000° 

thereby  completing  the  inverse-displacement  solutions  for  the  given  orientation 
problem. 


4.5  Exercises 

4.1  Shown  in  Fig.  10.3  is  the  kinematic  chain  of  one  of  the  six-dof  legs  of  a  flight 
simulator,  whose  architecture  is  defined  by  the  HD  parameters  of  Table  10.1. 
In  the  flight  simulator,  A4  is  the  moving  platform,  to  which  an  aircraft  cockpit 
is  rigidly  attached.  The  six-dof  motion  of  A4  is  controlled  by  means  of  the  six 
hydraulic  cylinders  identical  to  that  indicated  in  Fig.  10.3  as  a  prismatic  joint. 
Find  all  inverse  displacement  solutions  of  this  manipulator,  relating  the  pose 
of  A4  with  all  the  joint  variables. 

4.2  Modify  the  solution  procedure  of  Sect.  4.4  to  obtain  all  the  postures  of  a  FRR 
manipulator  that  give  the  same  EE  pose,  and  show  that  this  problem  leads  to  a 
quartic  polynomial  equation. 

4.3  Repeat  Exercise  4.2  as  pertaining  to  a  FRF  manipulator. 
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iZi 


? 


ff 


Fig.  4.23  A  six-revolute  robot  holding  a  heavy  tool 


Fig.  4.24  ABB-IRB  1000 
robotic  manipulator 


4.4  The  manipulator  appearing  in  Fig.  4.23  is  of  the  orthogonal  type,  with  a 
decoupled,  spherical  wrist,  and  a  regional  structure  consisting  of  two  parallel 
axes  and  one  axis  perpendicular  to  these  two.  Find  all  inverse  kinematics 
solutions  for  arbitrary  poses  of  the  EE  of  this  manipulator. 

4.5  Similar  to  the  manipulator  of  Fig.  4.23,  that  of  Fig.  4.24  is  of  the  orthogonal, 
decoupled  type,  except  that  the  latter  has  a  prismatic  pair.  For  an  arbitrary  pose 
of  its  EE,  find  all  inverse  displacement  solutions  of  this  manipulator. 

4.6  Derive  expressions  for  the  angle  of  rotation  and  the  unit  vector  parallel  to  the 
axis  of  rotation  of  matrices  Qf ,  as  introduced  in  Sect.  4.2. 
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Fig.  4.25  Geometric  information  taken  from  the  data  sheet  of  an  industrial  robot:  (a)  the  top  view; 
(b)  an  isometric  view;  (c)  front  view;  and  (d)  side  view 
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Fig.  4.26  Motoman-EA1400N  welding  robot:  (a)  top  view;  (b)  orthographic  projection;  (c)  side 
view;  (d)  view  A,  as  per  sideview;  (e)  view  B,  as  per  side  view.  All  dimensions  in  mm 


4.7  An  orthogonal  spherical  wrist  has  the  architecture  shown  in  Fig.  4.20,  with  the 
DH  parameters 


a4  =  90°,  ^5  =  90° 

A  frame  is  attached  to  its  EE  so  that  Zj  coincides  with  Z^.  Find  the 
(Cartesian)  orientation  that  can  be  attained  with  two  inverse  displacement 
solutions  0  /  and  On,  defining  the  two  distinct  postures,  that  lie  the  farthest 
apart.  Note  that  a  distance  between  two  manipulator  postures  can  be  defined 


4.5  Exercises 


183 


as  the  radical  of  the  quadratic  equation  yielding  the  two  inverse  kinematic 
solutions  of  the  wrist,  whenever  the  radical  is  positive.  Those  postures  giving 
the  same  EE  orientation  and  lying  farthest  from  each  other  are  thus  at  the  other 
end  of  the  spectrum  from  singularities,  where  the  two  postures  merge  into  a 
single  one.  Hence,  the  postures  lying  farthest  from  each  other  are  singularity- 
robust. 

4.8  Given  an  arbitrary  three-revolute  manipulator,  as  shown  in  Eig.4.11,  its 
singular  postures  are  characterized  by  the  existence  of  a  line  passing  through 
its  operation  point  about  which  the  moments  of  its  three  axes  vanish — see 
Exercise  3.3.  Note  that  this  condition  can  be  readily  applied  to  manipulators 
with  a  simple  architecture,  whereby  two  successive  axes  intersect  at  right 
angles  and  two  others  are  parallel.  However,  more  complex  architectures,  like 
that  of  the  manipulator  of  Eig.  4.15,  are  more  elusive  in  this  regard.  Eind  the 
line  passing  through  the  operation  point  and  intersecting  the  three  axes  of 
the  manipulator  of  Eig.  4.15  at  a  singularity.  Hint:  A  singular  posture  of  this 
manipulator  was  found  in  Example  4.4.2. 

4.9  Eor  the  Fanuc  Arc  Mate  120iB  robot  displayed  in  Eig.  4.25,  with  the  dimen¬ 
sions  in  mm  included  therein, 

(a)  Eind  its  Denavit-Hartenberg  parameters,  using  the  Z/  axes  suggested  in 
Eig.  4.25b. 

(b)  Apparently,  the  robot  under  study  is  of  the  decoupled  type.  Eind  all  its 
inverse-displacement  solutions  for  an  arbitrary  pose  of  its  end-effector, 
assuming  that  the  operation  point  is  located  at  a  point  of  coordinates 
[0.0,  100.0,  100.0]^  mm. 

4.10  Shown  in  Eig.  4.26  is  the  data  sheet  of  the  Motoman-EA1400N  welding  robot. 
Under  the  assumption  that  the  operation  point  of  the  robot  is  located  along 
the  axis  of  the  6th  joint,  on  the  flange  indicated  in  View  A,  produce  a  table 
with  the  Denavit-Hartenberg  parameters  of  the  robot.  N.B.:  all  lengths  are 
indicated  in  mm.  It  is  strongly  recommended  to  sketch  the  robot  at  an  arbitrary 
posture  in  order  to  ease  the  definition  of  the  DH  coordinate  frames.  Is  this 
robot  decoupled? 


Chapter  5 

Kinetostatics  of  Serial  Robots 


5.1  Introduction 

Kinetostatics  is  understood  here  as  the  study  of  the  interplay  between  the  feasible 
twists  of  and  the  constraint  wrenches  acting  on  the  various  rigid  bodies  of  a 
mechanical  system,  when  the  system  moves  under  static,  conservative  conditions. 
The  feasible  twists  of  the  various  rigid  bodies,  or  links,  are  those  allowed  by  the 
constraints  imposed  by  the  robot  joints.  The  constraint  wrenches  are,  in  turn,  the 
reaction  forces  and  moments  exerted  on  a  link  by  the  links  to  which  that  link  is 
coupled  by  means  of  joints.  The  subject  of  this  chapter  is  the  kinetostatics  of  serial 
robots,  with  focus  on  six-axis  manipulators.  By  virtue  of  the  duality  between  the 
kinematic  and  the  static  relations  in  the  mechanics  of  rigid  bodies,  as  outlined  in 
Sect.  3.7,  the  derivation  of  the  kinematic  relations  is  discussed  in  detail,  the  static 
relations  following  from  the  former. 

We  derive  first  the  relation  between  the  twist  of  the  robot  EE  and  the  set  of 
joint  rates,  which  is  given  by  a  linear  transformation  induced  by  the  robot  Jacobian 
matrix.  Once  the  foregoing  relation  is  established  for  a  general  six-joint  robot,  the 
relation  between  the  static  wrench  exerted  by  the  environment  on  the  EE  and  the 
balancing  joint  torques  is  derived  by  duality.  Special  robotic  architectures  are  given 
due  attention.  Decoupled  and  planar  architectures  are  treated  as  special  cases  of 
six-joint  robots.  The  fundamental  problem  of  singularities  arising  from  a  singular 
robot  Jacobian  in  decoupled  manipulators  is  given  due  attention  as  well.  Two  types 
of  singularities  are  discussed  here  for  the  regional  structure  of  decoupled  robots. 
As  a  follow-up  to  the  singularity  analysis  of  this  structure,  its  three-dimensional 
workspace  is  derived.  An  algorithm  is  proposed  for  the  display  of  this  workspace  as 
pertaining  to  general  regional  structures  whose  inverse  displacement  analysis  leads 
to  a  quartic  polynomial. 
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The  chapter  closes  with  a  section  on  kinetostatic  performance  indices.  The  pur¬ 
pose  of  these  indices  is  twofold:  They  are  needed  in  robot  design  to  help  the  designer 
best  dimension  the  links  of  the  robot  in  the  early  stages  of  the  design  process,  prior  to 
the  elasto  static  and  the  e  las  tody  namic  design  stages.  These  indices  are  also  needed 
in  the  control  of  a  given  robot  to  ensure  an  acceptable  kinetostatic  performance 
under  feedback  control.  One  third,  pragmatic  application  of  these  indices  is  the 
comparison  of  various  candidate  robots  when  a  robotic  facility  is  being  planned. 

Elastostatic  design  pertains  to  the  structural  design  of  a  robot  to  ensure  that  the 
links  and  the  joint  mechanical  transmissions  will  be  able  to  withstand  the  static  loads 
that  arise  when  the  robot  is  in  operation.  This  aspect  of  design  is  usually  conducted 
under  the  assumption  that  all  structural  elements  operate  within  the  linearly  elastic 
range,  and  is  valid  at  a  specific  robot  posture.  Elastodynamic  design  considers  the 
inertial  load  of  the  structural  elements  while  accounting  for  link  flexibility,  which 
gives  rise  to  mechanical  vibration.  The  main  concern  here  is  avoiding  resonance 
under  linear  dynamical  conditions  or  limit  cycles  under  nonlinear  conditions.  Both 
elastostatics  and  elastodynamics  lie  beyond  the  scope  of  the  book,  and  hence,  will 
not  be  considered  here. 


5.2  Velocity  Analysis  of  Serial  Manipulators 

The  relationships  between  the  prescribed  twist  of  the  EE,  also  referred  to  as  the 
Cartesian  velocity  of  the  manipulator,  and  the  corresponding  joint-rates  are  derived 
in  this  section.  Eirst,  a  serial  /7-axis  manipulator  containing  only  revolute  pairs 
is  considered.  Then,  relations  associated  with  prismatic  pairs  are  introduced,  and 
finally,  the  joint  rates  of  six-axis  manipulators  are  calculated  in  terms  of  the  EE 
twist.  Particular  attention  is  given  to  decoupled  manipulators,  for  which  simplified 
velocity  relations  are  derived. 

We  consider  here  the  manipulator  of  Eig.  5.1,  in  which  a  joint  coordinate  Of, 
a  joint  rate  Ot ,  and  a  unit  vector  e,  are  associated  with  each  revolute  axis.  The 
Xf,  Yi,  Zi  coordinate  frame,  attached  to  the  (/  —  l)st  link,  is  not  shown,  but  its 

62 

On 

A, 


Fig.  5.1  General  /z-axis  manipulator 
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origin  Ot  is  indicated.  If  the  angular- velocity  vector  of  the  /th  link  is  denoted  by  o)/ , 
then  we  have,  from  Fig.  5.1, 

0)0  =  0 

0)1  =  ^101 

•  • 

0)2  =  ^101  +  ^2^2 


•  •  • 

+  ^2^2  +  •  •  •  +  (5.1) 

and  if  the  angular  velocity  of  the  EE  is  denoted  by  o) ,  then 

n 

0)  =  (On  =  0\^\  +  62^2  +  •  •  •  +  On^n  —  ^  ^ 

1 


Likewise,  from  Fig.  5.1,  one  readily  derives 

p  =  ai  +  a2  H - h  a„  (5.2) 

where  p  denotes  the  position  vector  of  point  P  of  the  EE.  Moreover,  notice  that  all 
vectors  of  the  above  equation  must  be  expressed  in  the  same  frame;  otherwise,  the 
addition  would  not  be  possible — vector  a^  was  defined  as  expressed  in  the  i  th  frame 
in  Eq.  (4.3c).  Upon  differentiating  both  sides  of  Eq.  (5.2),  we  have 

p  =  ai  +  a2  H - h  a„  (5.3) 

Since  vector  a^  is  fixed  to  the  i  th  link, 

ki  =  (Oi  X  a^,  i  =  (5.4) 

Furthermore,  substitution  of  Eqs.  (5.1)  and  (5.4)  into  Eq.  (5.3)  yields 


•  •  • 

p  =  diCi  X  ai  +  (0iei  +  62^2)  X  32  + 


•  •  • 

+  (0101  +  02^2  +  •  •  •  +  On^n)  ^ 


(5.5) 


which  can  be  readily  rearranged  as 


p  —  0101  X  (ai  +  a2  +  •  •  •  +  a^i)  +  02^2  x  (a2  +  a3  +  •  •  •  +  a„) 
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Now  vector  r/  is  defined  as  that  joining  Of  with  P ,  directed  from  the  former  to 
the  latter,  as  depicted  in  Fig.  5.1,  i.e., 

r,-  =  a,-  +  a/+i  H - h  a„  (5.6) 

and  hence,  p  can  be  rewritten  as 

n 

p  =  ^  fte;  X  r,' 

1 

Further,  let  A  and  B  denote  the  3  x  matrices  defined  as 

A  =  [d  62  •••  e„]  (5.7a) 

B  =  [ci  X  ri  62  X  r2  •  •  •  e„  X  r„]  (5.7b) 

the  /7 -dimensional  joint-rate  vector  0  being  defined,  in  turn,  as 

Thus,  (o  and  p  can  be  expressed  in  a  more  compact  form  as 

(o  =  AO ,  ^  =  BO 

the  twist  of  the  EE  being  defined,  in  turn,  as 


(5.8) 


The  EE  twist  is  thus  linearly  related  to  the  joint-rate  vector  0 ,  i.e., 

JO  =  t  (5.9) 

where  J  is  the  Jacobian  matrix,  or  Jacobian,  for  brevity,  of  the  manipulator  under 
study,  first  introduced  by  Whitney  (1972).  The  Jacobian  is  defined  as  the  6x/i  matrix 
shown  below: 


(5.10a) 


or 


ei  02  •  •  •  e„ 

01  X  ri  02  X  r2  •  •  •  e„  X  r„ 


(5.10b) 


Apparently,  an  alternative  definition  of  the  foregoing  Jacobian  matrix  can  be 
given  as 


9t 
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Moreover,  ifj,  denotes  the  i  th  column  of  J,  one  has 


e,- 

e,-  X  r,- 


It  is  noteworthy  that  if  the  axis  of  the  /th  revolute  is  denoted  by  IZi,  then  j/  is 
nothing  but  the  Pliicker  array  of  that  line,  with  the  moment  of  IZi  being  taken  with 
respect  to  the  operation  point  P  of  the  EE. 

On  the  other  hand,  if  the  /th  pair  is  prismatic,  then  the  (/  —  l)st  and  the  /th 
links  have  the  same  angular  velocity,  for  a  prismatic  pair  does  not  allow  any  relative 
rotation.  However,  vector  a/  joining  the  origins  of  the  /  th  and  (/  +  l)st  frames  is  no 
longer  of  constant  magnitude,  but  undergoes  a  change  of  magnitude  along  the  axis 
of  the  prismatic  pair.  This  can  be  appreciated  by  looking  at  Eig.  4.6,  with  bi  playing 
the  role  of  joint  variable.  Hence, 

<y,-  =  ki  =  «,_i  X  a,-  +  Z),e,- 


One  can  readily  prove,  in  this  case,  that 

•  •  •  •  • 

(O  =  0\^l  +  62^2  +  •  •  •  +  —  —  l  +  l  +  •  •  •  +  Oyi^n 

•  •  •  • 

p  =  diCi  X  ri  +  02^2  X  r2  H - h  x  r,_i  +  biCi 

•  • 

+  X  r/+i  +  •  •  •  +  Ofi^n  X  a„ 


from  which  it  is  apparent  that  the  relation  between  the  twist  of  the  EE  and  the  joint- 
rate  vector  is  formally  identical  to  that  appearing  in  Eq.  (5.9)  if  vector  0  is  now 
defined  as 

0  =  [Oi  62  ' "  Oi-i  hi  ft+i  •••  0n\ 


the  /  th  column  of  J  then  changing  to 


(5.11) 


Note  that  the  Pliicker  array  of  the  axis  of  the  /  th  joint,  if  prismatic,  is  that  of  a  line 
at  infinity  lying  in  a  plane  normal  to  the  unit  vector  ,  as  defined  in  Eq.  (3.33). 

If,  in  general,  denotes  the  Jacobian  defined  for  a  point  A  of  the  EE  and  J b 
that  defined  for  another  point  B,  then  the  relation  between  and  J5  is 


iB  =  UJ^ 


(5.12a) 


where  the  6x6  matrix  U  is  defined  as 


1  O 
A-B  1 


U  = 


(5.12b) 
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while  A  and  B  are  now  the  cross-product  matrices  of  the  position  vectors  a  and  b 
of  points  A  and  B,  respectively.  Moreover,  this  matrix  U  is  identical  to  the  matrix 
defined  under  the  same  name  in  Eq.  (3.29b),  and  hence,  it  belongs  to  the  6x6 
unimodular  group,  i.e.,  the  group  of  6  x  6  matrices  whose  determinant  is  unity. 
Thus, 


detCJfi)  =  det(J^)  (5.13) 

We  have  then  proven  the  result  below: 

Theorem  5.2.1.  The  determinant  of  the  Jacobian  matrix  of  a  six-axis  manipulator 
is  not  affected  under  a  change  of  operation  point  of  the  EE. 

Note,  however,  that  the  Jacobian  matrix  itself  changes  under  a  change  of 
operation  point.  By  analogy  with  the  twist-  and  the  wrench-transfer  formulas, 
Eq.  (5.12a)  can  be  called  the  Jacobian-transfer  formula. 

In  particular,  for  six-axis  manipulators,  J  is  a  6  x  6  matrix.  Whenever  this  matrix 
is  nonsingular,  Eq.  (5.9)  can  be  solved  for  0 ,  namely, 

9  =  J“4  (5.14) 

Equation  (5.14)  is  only  symbolic,  for  the  inverse  of  the  Jacobian  matrix  need 
not  be  computed  explicitly.  Indeed,  in  the  general  case,  matrix  J  cannot  be 
inverted  symbolically,  and  hence,  0  is  computed  using  a  numerical  procedure, 
the  most  suitable  one  being  the  Gauss -elimination  algorithm,  also  known  as  LU 
decomposition  (Golub  and  Van  Loan  1989).  Gaussian  elimination  produces  the 
solution  by  recognizing  that  a  system  of  linear  equations  is  most  easily  solved 
when  it  is  in  either  upper-  or  lower-triangular  form.  To  exploit  this  fact,  matrix  J 
is  decomposed  into  the  unique  L  and  U  factors  in  the  form: 

J  =  LU  (5.15a) 

where  L  is  lower-  and  U  is  upper-triangular.  Moreover,  they  have  the  forms 


1 

0  ••• 

0 

hi 

1  ••• 

• 

0 

hi  •  •  • 

1_ 

~Uii 

Ui2  •  • 

•  Uin 

0 

U22  •  • 

•  Uln 

_  0 

0  •• 

•  • 

(5.15b) 


(5.15c) 
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where  in  the  particular  case  at  hand,  n  =  6.  Thus,  the  unknown  vector  of  joint  rates 
can  now  be  computed  from  two  triangular  systems,  namely, 

Ly  =  t,  W  =  y  (5.16) 

The  above  equations  are  then  solved,  first  for  y  and  then  for  0 ,  by  application  of 
only  forward  and  backward  substitutions,  respectively.  The  LU  decomposition  of 
diU  X  n  matrix  requires  multiplications  and  A'^  additions,  whereas  the  forward 
substitution  needed  in  solving  the  lower-triangular  system  of  Eq.  (5.16)  requires 
multiplications  and  additions.  Moreover,  the  backward  substitution  needed  in 
solving  the  upper-triangular  system  of  Eq.  (5.16)  requires  M^'  multiplications  and 
A'!^'  additions.  These  figures  are  (Dahlquist  and  Bjorck  1974) 


M'  = 

n 

n  n 

3  +  2+6’ 

A'  — 

n 

3  3 

(5.17a) 

M"  = 

n 

n{n  — 

A"  — 

n{n  —  \) 

(5.17b) 

2 

2 

M'"  = 

n 

n{n 

A'"  — 

n{n  —  \) 

(5.17c) 

2 

2 

Thus,  the  solution  of  a  system  of  n  linear  equations  in  n  unknowns,  using  the 
LU-decomposition  method,  can  be  accomplished  with  multiplications  and  A^ 
additions,  as  given  below  (Dahlquist  and  Bjorck  1974): 

M„  =  -(In^  +  9n  +  1),  A„  =  -(n^  +  3n-4)  (5.18a) 

6  3 

Hence,  the  velocity  resolution  of  a  six-axis  manipulator  of  arbitrary  architecture 
requires  multiplications  and  A(^  additions,  i.e.. 

Me  =  127,  Ae  =  100  (5.18b) 


5.2.7  Decoupled  Manipulators 

Decoupled  manipulators  allow  an  even  simpler  velocity  resolution.  Eor  manipula¬ 
tors  with  this  type  of  architecture,  it  is  more  convenient  to  deal  with  the  velocity  of 
the  center  C  of  the  wrist  than  with  that  of  the  operation  point  P .  Thus,  one  has 


tc  = 
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where  tc  is  defined  as 


CO 

c 


and  can  be  obtained  from  tp  =  [co^ ,  using  the  twist-transfer  formula  given  by 
Eqs.  (3.83a)  and  (3.83b)  as 


1  O 
P-C  1 


with  C  and  P  defined  as  the  cross-product  matrices  of  the  position  vectors  c  and  p, 
respectively. 

Since  C  is  on  the  last  three  joint  axes,  its  velocity  is  not  affected  by  the  motion 
of  the  last  three  joints,  and  hence,  we  can  write 


•  •  • 

c  =  X  ri  +  0000  X  ro  + 


where,  in  the  case  of  a  decoupled  manipulator,  vector  r/  is  defined  as  that  directed 
from  Oi  to  C .  On  the  other  hand,  we  have 

•  ••••• 

(O  =  -\-  62^2  +  ^3^3  +  ^4^4  +  ^5^5  +  ^6^6 


and  thus,  the  Jacobian  takes  on  the  following  simple  form 


J  = 


Jii  J12 

J21  O 


(5.19) 


where  O  denotes  the  3x3  zero  matrix,  the  other  3x3  blocks  being  given  below, 
for  manipulators  with  revolute  pairs  only,  as 


Jii  —  [ei  02  03] 

J12  =  [^4  05  05] 

J21  =  [ei  X  ri  02  X  Y2  63  X  ra] 


(5.20a) 

(5.20b) 

(5.20c) 


Further,  vector  0  is  partitioned  accordingly: 


Oa 

Ow 


• 

r^ii 

• 

• 

I 

I _ 

Oa  ^ 

62 

05 

I 

I _ 

I 

o^ 

where 
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Henceforth,  the  three  components  of  ^  ^  will  be  referred  to  as  the  arm  rates,  whereas 
those  of  will  be  called  the  wrist  rates.  Now  Eqs.  (5.9)  can  be  written,  for  this 
particular  case,  as 


+  Jl2^w  —  ^  (5.21a) 

J21^a  =  c  (5.21b) 

from  which  the  solution  is  derived  successively  from  the  two  systems  of  three 
equations  and  three  unknowns  that  follow: 

hiK  =  c  (5.22a) 

Jl2^w  =  ^  “  Jll^a  (5.22b) 


From  the  general  expressions  (5.17),  then,  it  is  apparent  that  each  of  the  foregoing 
systems  can  be  solved  with  the  numbers  of  operations  shown  below: 


M3  =  23,  As  =  14 


Since  the  computation  of  the  right-hand  side  of  Eq.  (5.22b)  requires,  additionally, 
nine  multiplications  and  nine  additions,  the  total  numbers  of  operations  required  to 
perform  one  joint-rate  resolution  of  a  decoupled  manipulator.  My  multiplications 
and  Ay  additions,  are  given  by 


My  =  55,  Ay  =  37  (5.23) 

which  are  fairly  low  figures  and  can  be  performed  in  a  matter  of  microseconds  using 
a  modern  processor. 

It  is  apparent  from  the  foregoing  kinematic  relations  that  Eq.  (5.22a)  should  be 

•  • 

first  solved  for  0 a',  with  this  value  available,  Eq.  (5.22b)  can  then  be  solved  for  0^. 
We  thus  have,  symbolically. 


6  a  =  Jj/c  (5.24) 

K=3ni(»-inK)  (5.25) 

Now,  if  we  recall  the  concept  of  reciprocal  bases  introduced  in  Sect.  2.7.1,  the  above 
inverses  can  be  represented  explicitly.  Indeed,  let 


A21  =  det(J2i)  =  (ei  X  n)  X  (e2  x  r2)  •  (63  x  rs) 
Ai2  =  det(Ji2)  =  64  X  65  •  eg 


(5.26) 

(5.27) 
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Then, 


Therefore, 


and,  if  we  let^ 
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(5.28) 


(5.29) 


(5.30a) 


(5.30b) 


(5.30c) 


5.3  Jacobian  Evaluation 

The  evaluation  of  the  Jacobian  matrix  of  a  manipulator  with  n  revolutes  is  discussed 
in  this  subsection,  the  presence  of  a  prismatic  pair  leading  to  simplifications  that 
will  be  outlined.  Our  aim  here  is  to  devise  algorithms  requiring  a  minimum  number 
of  operations,  for  these  calculations  are  needed  either  in  real-time  or  in  off-line 
applications  when  these  require  Jacobian  evaluations  at  massive  numbers  of  poses, 
of  the  order  of  millions.^  We  assume  at  the  outset  that  all  joint  variables  producing 
the  desired  EE  pose  are  available.  We  divide  this  section  into  two  subsections,  one 
for  the  evaluation  of  the  upper  part  of  the  Jacobian  matrix  and  one  for  the  evaluation 
of  its  lower  part. 


^  TU  is  read  varpi. 

^One  such  application  occurs  in  path  planning  for  machining  operations.  Robotmaster  offers  a 
feature  that  allows  the  display  of  maps  of  the  Jacobian  condition  number,  introduced  in  Sect.  5.8. 
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5.3.1  Evaluation  of  Submatrix  A 

The  upper  part  A  of  the  Jacobian  matrix  is  composed  of  the  set  {e^  }",  our  aim 
here  being  the  calculation  of  these  unit  vectors.  Note,  moreover,  that  vector  [e,  ]i 
is  nothing  but  the  last  column  of  P,_i  =  Qi  our  task  then  being  the 

calculation  of  these  matrix  products.  According  to  the  DH  notation, 

[e,],-  =  [0  0  1]^ 


Hence,  [ei  ]i  is  available  at  no  cost.  However,  each  of  the  remaining  [e,  ]i  vectors, 
for  /  =  2, ...,«,  is  obtained  as  the  last  column  of  matrices  P/-i.  The  recursive 
calculation  of  these  matrices  is  described  below: 

Pi  ^Qi 
P2  =  P1Q2 


and  hence,  a  simple  algorithm  follows: 

Pi  ^  Qi 

For  i  =  2  to  n  do 

P.  ^  P -iQ. 

enddo 

Now,  since  Pi  is  identical  to  Qi ,  the  first  product  appearing  in  the  do-loop,  P1Q2, 
is  identical  to  Q1Q2,  whose  two  factors  have  a  special  structure.  The  computation 
of  this  product,  then,  requires  special  treatment,  which  warrants  further  discussion 
because  of  its  particular  features.  From  the  structure  of  matrices  Q/,  as  displayed  in 
Eq.  (4.1e),  we  have 


COS  0i  —Xi  sin  0i 

/Xi  sin  0i 

cos  O2  —X2  sin  O2  /X2  sin  O2 

P2  = 

sin^i  Aicos^i 

— /xi  cos  0i 

sin  O2  A 2  cos  O2  — /X2  cos  O2 

1 

0 

Ai  _ 

_  0  /X2  A2  _ 

The  foregoing  product  is  calculated  now  by  first  computing  the  products  Ai  A2,  A1/X2, 
/X1/X2,  and  A2/X1,  which  involve  only  constant  quantities,  these  terms  thus  being 
posture-independent.  Thus,  in  tracking  a  prescribed  Cartesian  trajectory,  the  manip¬ 
ulator  posture  changes  continuously,  and  hence,  its  joint  variables  also  change. 
However,  its  DH  parameters,  those  defining  its  architecture,  remain  constant. 
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Therefore,  the  four  above  products  remain  constant  as  well  and  are  computed  prior 
to  tracking  a  trajectory,  i.e.,  off-line.  In  computing  these  products,  we  store  them  as 

Ai2  =  A1A2,  /X21  =  A1/X2,  /X12  =  /X1/X2,  A21  =  A2/X1 


Next,  we  perform  the  on-line  computations.  First,  let^ 


G 

X 

V 

u 

V 


X 1  sin  O2 
sin  0i  cos  O2 
cos  0i  cos  O2 
cos  0i  sin  ^2  +  A 1  r 
sin  0i  sin  02  —  ^  1  ^ 


and  hence. 


u  — O' sin  01  —  A2i/ + /X12  sin  01  /X21/ +  A21  sin  0i 
r  +  O'  cos  01  —X2V  —  /X12  cos  01  /X2r’  —  A21  cos  0i 
/Xisin02  A21  cos  02  +  /X21  -/X12COS02  +  Ai2 


As  the  reader  can  verify,  the  foregoing  calculations  consume  20  multiplications  and 
ten  additions.  Now,  we  proceed  to  compute  the  remaining  products  in  the  foregoing 
do-loop. 

Here,  notice  that  the  product  P^-iQ/,  for  3  <  i  <  n,  can  be  computed 
recursively,  as  described  below:  Let  P/_i  and  P/  be  given  as 


Pn  Pu  Pi3 
P21  P22  P23 
_P3l  P32  P33_ 


fit'll  Pu  P'l3~ 

P;  =  P21  P22  P23 

-P3I  P32  P33- 

Now  matrix  P^  is  computed  by  first  defining 

Ui  =  Pn  sin  Oi  -  pu  cos  Of 
Vi  =  P21  sin  Oi  -  P22  cos  Oi 

^i  =  P3\  sin  Oi  -  P32  cos  Oi  (5.31a) 


^Although  V  and  v  look  similar,  they  should  not  be  confused  with  each  other,  the  former  being  the 
lowercase  Greek  letter  upsilon.  As  a  matter  of  fact,  no  confusion  should  arise,  because  upsilon  is 
used  only  once,  and  does  not  appear  further  in  the  book. 
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and 


p'n  =  PncosOi  +  /^nsin^,- 

p'\2  ~  ~UiXi  +  PnP^i 

p'\2  ~  ^it^i  +  PnXi 

P21  =  P21  cos  0i  +  P22  sin  Oi 

P22  =  -^ih  +  P23l^i 

P22  =  Villi  +  p23Xi 

P31  =  P31  cos  0i  +  P22  sin  0i 

P32  =  +  PiiP'i 

p'22  =  Willi  +  PiiXi  (5.31b) 

Computing  ui,  Vf,  and  w/  requires  six  multiplications  and  three  additions, 
whereas  each  of  the  p'-j  entries  requires  two  multiplications  and  one  addition. 
Hence,  the  computation  of  each  matrix  requires  24  multiplications  and  12 
additions,  the  total  number  of  operations  required  to  compute  the  n  —  2  products 
{  Fj  }2~^  thus  being  24{n—2)  +  20  =  24/1—28  multiplications  and  12 (/i— 2) +  10  = 
12/7  —  14  additions,  for  n  >2.  Moreover,  Pi,  i.e.,  Qi,  requires  four  multiplications 
and  no  additions,  the  total  number  of  multiplications  and  additions  A  a  required 
to  compute  matrix  A  thus  being 


Ma  =  24n  -  24,  Aa  =  I2n  -  14  (5.32) 

Before  concluding  this  subsection,  a  remark  is  in  order:  The  reader  may  realize 
that  Fn  is  nothing  but  Q,  and  hence,  the  same  reader  may  wonder  whether  we  could 
not  save  some  operations  in  the  foregoing  computations  by  stopping  the  above 
recursive  algorithm  at  /7  —  1 ,  rather  than  at  n .  This  is  not  a  good  idea,  for  the  above 
equality  holds  if  and  only  if  the  manipulator  is  capable  of  tracking  perfectly  a  given 
trajectory.  However,  reality  is  quite  different,  and  errors  are  always  present  when 
tracking.  As  a  matter  of  fact,  the  mismatch  between  P„  and  Q  is  very  useful  in 
estimating  orientation  errors,  which  are  then  used  in  a  feedback-control  scheme  to 
synthesize  the  corrective  signals  that  are  intended  to  correct  those  errors. 


5.3.2  Evaluation  of  Submatrix  B 

The  computation  of  submatrix  B  of  the  Jacobian  is  studied  here.  This  submatrix 
comprises  the  set  of  vectors  {  e,  x  r,  .  We  thus  proceed  first  to  the  computation 
of  vectors  r/ ,  for  /  =  1 , . . . ,  // ,  which  is  most  efficiently  done  using  a  recursive 
scheme,  similar  to  that  of  Horner  for  polynomial  evaluation  (Henrici  1964),  namely. 
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[rele  ^  [aele 

For  i  =  5  to  1  do 

[  ]  i  ^  ]  i  “1“  Q;  [  + 1  ]  z  + 1 

enddo 


In  the  foregoing  algorithm,  a  simple  scheme  is  introduced  to  perform  the  product 
Q,[r,+i],+i  economically:  if  we  let  [r,+i],+i  =  [n,  ra,  r3]^,then 


where 


cos  Oi  —Xi  sin  Oi  /X/  sin  Oi 

~ri~ 

sin  Oi  Xi  cos  Oi  —pti  cos  Oi 

r2 

1 

o 

_ 1 

J3_ 

ri  cos  Oi  —  u  sin  Of 
ri  sin  Oi  +  u  cos  Of 
r2lii  +  r^Xi 


(5.33a) 


u  =  raA;  -  Titii 


(5.33b) 


Therefore,  the  product  of  matrix  Q,  by  an  arbitrary  vector  consumes  eight  multipli¬ 
cations  and  four  additions. 

Furthermore,  each  vector  [a^  ], ,  for  /  =  I, ...  ,n,  requires  two  multiplications 
and  no  additions,  as  made  apparent  from  their  definitions  in  Eq.  (4.3b).  Moreover, 
from  the  foregoing  evaluation  of  Q;[r,+i  ]/+i,  it  is  apparent  that  each  vector  r,  , 
in  frame  ,  is  computed  with  ten  multiplications  and  seven  additions — two  more 
multiplications  are  needed  to  calculate  each  vector  [a^  ],  and  three  more  additions 
are  required  to  add  the  latter  to  vector  Qj  [r/+i]/+i — the  whole  set  of  vectors 
{r/  thus  being  computed,  in  /’/-coordinates,  with  10(/i  —  l)  +  2  =  10/1  —  8 
multiplications  and  7 (n  —  l)  additions,  where  one  coordinate  transformation,  that  of 
ri ,  is  not  counted,  since  this  vector  is  computed  directly  in  . 

Now  we  turn  to  the  transformation  of  the  components  of  all  the  foregoing  vectors 
into  /"i -coordinates.  First,  note  that  we  can  proceed  now  in  two  ways:  in  the  first, 
we  transform  the  individual  vectors  e/  and  r,  from  /’/-  into  /’i -coordinates  and  then 
compute  their  cross  product;  in  the  second,  we  first  perform  the  cross  products  and 
then  transform  each  of  these  products  into  /"i -coordinates.  It  is  apparent  that  the 
second  approach  is  more  efficient,  which  is  why  we  choose  it  here. 

In  order  to  calculate  the  products  e,  x  r/  in  /^  -coordinates,  we  let  [r/  ]/  = 
[pi,  P2,  P3  V- Moreover,  [e,-  ]/  =  [0,  0,  1  ]^,  and  hence. 


-p2 

Pi 

0 


[e/  X  Yi  ]i  = 
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which  is  thus  obtained  at  no  cost.  Now,  the  transformation  from  Ti-  into  T\- 
coordinates  is  simply 


[e,-  X  r;  ]i  =  P,-i[e,-  x  r,-  ],•  (5.34) 

In  particular,  [ei  xri]i  needs  no  transformation,  for  its  two  factors  are  given 
in  -coordinates.  The  -components  of  the  remaining  cross  products  are  com¬ 
puted  using  the  general  transformation  of  Eq.  (5.34).  In  the  case  at  hand,  this 
transformation  requires,  for  each  /,  six  multiplications  and  three  additions,  for 
this  transformation  involves  the  product  of  a  full  3x3  matrix,  P/-i,  by  a  three- 
dimensional  vector,  0;  X  ,  whose  third  component  vanishes.  Thus,  the  computation 
of  matrix  B  requires  Mb  multiplications  and  Ab  additions,  as  given  below: 

Mb  =  16^  -  14,  Ab  =  10(w  -  1)  (5.35) 

In  total,  then,  the  evaluation  of  the  complete  Jacobian  requires  Mj  multiplica¬ 
tions  and  A  j  additions,  namely. 


Mj  =  40w  -  38,  Aj  =  Tin  -  24  (5.36) 

In  particular,  for  a  six-revolute  manipulator,  these  figures  are  202  multiplications 
and  108  additions. 

Now,  if  the  manipulator  contains  some  prismatic  pairs,  the  foregoing  figures 
diminish  correspondingly.  Indeed,  if  the  /th  joint  is  prismatic,  then  the  /th  column 
of  the  Jacobian  matrix  changes  as  indicated  in  Eq.  (5.11).  Hence,  one  cross- 
product  calculation  is  spared,  along  with  the  associated  coordinate  transformation. 
As  a  matter  of  fact,  as  we  saw  above,  the  cross  product  is  computed  at  no  cost 
in  local  coordinates,  and  so  each  prismatic  pair  of  the  manipulator  reduces  the 
foregoing  numbers  of  operations  by  only  one  coordinate  transformation,  i.e.,  by 
ten  multiplications  and  seven  additions. 


5.4  Singularity  Analysis  of  Decoupled  Manipulators 

In  performing  the  computation  of  the  joint  rates  for  a  decoupled  manipulator,  it  was 
assumed  that  neither  J12  nor  J21  is  singular.  If  the  latter  is  singular,  then  none  of 
the  joint  rates  can  be  evaluated,  even  if  the  former  is  nonsingular.  However,  if  J21  is 
nonsingular,  then  Eq.  (5.21a)  can  be  solved  for  the  arm  rates  even  if  J12  is  singular. 
Each  of  these  sub-Jacobians  is  analyzed  for  singularities  below. 

We  will  start  analyzing  J21,  whose  singularity  determines  whether  any  joint-rate 
resolution  is  possible  at  all.  Eirst,  we  note  from  Eq.  (5.20c)  that  the  columns  of  J21 
are  the  three  vectors  ei  x  ri ,  02  x  r2,  and  03  x  r3 .  Hence,  J21  becomes  singular  if  either 
these  three  vectors  become  coplanar  or  at  least  one  of  them  vanishes.  Eurthermore, 
neither  the  relative  layout  of  these  three  vectors  nor  their  magnitudes  change  if 
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the  manipulator  undergoes  a  motion  about  the  first  revolute  axis  while  keeping 
the  second  and  the  third  revolute  axes  locked.  This  means  that  0i  does  not  affect 
the  singularity  of  the  manipulator,  a  result  that  can  also  be  derived  from  invariance 
arguments — see  Sect.  2.6 — and  by  noticing  that  singularity  is,  indeed,  an  invariant 
property.  Hence,  whether  a  configuration  is  singular  or  not  is  independent  of  the 
viewpoint  of  the  observer,  a  change  in  0i  being  nothing  but  a  change  of  viewpoint. 
The  same  argument  holds  for  bi  in  cases  where  the  first  joint  is  prismatic. 

The  singularity  of  a  three-revolute  arm  for  positioning  tasks  was  analyzed  by 
Burdick  (1995),  upon  recognizing  that  (a)  given  three  arbitrary  lines  in  space,  the 
three  revolute  axes  in  our  case,  it  is  always  possible  to  find  a  set  of  lines  that 
intersects  all  three,  and  (b)  the  moments  of  the  three  lines  about  any  point  on  the 
intersecting  line  are  all  zero.  As  a  matter  of  fact,  the  locus  of  those  lines  is  a  quadric 
ruled  surface,  namely,  a  one-sheet  hyperboloid — see  Exercise  3.3.  Therefore,  if  the 
endpoint  of  the  third  moving  link  lies  in  this  quadric,  the  manipulator  is  in  a  singular 
posture,  and  velocities  of  C  along  the  intersecting  line  cannot  be  produced.  This 
means  that  the  manipulator  has  lost,  to  Sl  first  order,  one  degree  of  freedom.  Here  we 
emphasize  that  this  loss  is  meaningful  only  at  a  first  order  because,  in  fact,  a  motion 
along  that  intersecting  line  may  still  be  possible,  provided  that  the  full  nonlinear 
relations  of  Eq.  (4.16)  are  considered.  If  such  a  motion  is  at  all  possible,  however, 
then  it  is  so  only  in  one  direction,  as  we  shall  see  in  Case  2  below.  Motions  in  the 
opposite  direction  are  not  feasible  because  of  the  rigidity  of  the  links. 

We  will  illustrate  the  foregoing  concepts  as  pertaining  to  the  most  common  types 
of  industrial  manipulators,  i.e.,  those  of  the  orthogonal  type.  In  these  cases,  two 
consecutive  axes  either  intersect  at  right  angles  or  are  parallel;  most  of  the  time,  the 
first  two  axes  intersect  at  right  angles  and  the  last  two  are  parallel.  Below  we  study 
each  of  these  cases  separately. 

Case  1:  Two  consecutive  axes  intersect  and  C  lies  in  their  plane.  Here,  the  ruled 
hyperboloid  containing  the  lines  that  intersect  all  three  axes  degenerates  into  a 
plane,  namely,  that  of  the  two  intersecting  axes.  Eor  conciseness,  let  us  assume 
that  the  first  two  axes  intersect,  but  the  derivations  are  the  same  if  the  intersecting 
axes  are  the  last  two.  Moreover,  let  Ou  be  the  intersection  of  the  first  two  axes, 
ni2  being  the  plane  of  these  axes  and  ni2  its  normal.  If  we  recall  the  notation 
adopted  in  Sect.  5.2,  we  have  now  that  the  vector  directed  from  Ou  to  C  can  be 
regarded  as  both  ri  and  r2.  Eurthermore,  ei  x  ri  and  02  x  r2  (=  62  x  rO  are  both 
parallel  to  1112.  Hence,  the  first  two  axes  can  only  produce  velocities  of  C  in  the 
direction  of  1112.  As  a  consequence,  velocities  of  C  in  ni2  and  perpendicular  to 
03  xr3  cannot  be  produced  in  the  presence  of  this  singularity.  The  set  of  unfeasible 
velocities,  then,  lies  in  a  line  normal  to  1112  and  03  x  r3,  whose  direction  is  the 
geometric  representation  of  the  null  space  of  .  Likewise,  the  manipulator  can 
withstand  forces  applied  at  C  in  the  direction  of  the  same  line  purely  by  reaction 
wrenches,  i.e.,  without  any  motor  torques.  The  last  issue  falls  into  the  realm  of 
manipulator  statics,  upon  which  we  will  elaborate  in  Sect.  5.6. 

We  illustrate  this  singularity,  termed  here  shoulder  singularity,  in  a  manipulator 
with  the  architecture  of  Eig.  4.3,  as  postured  in  Eig.  5.2.  In  this  figure,  the  line 
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Fig.  5.2  Shoulder  singularity 
of  the  Puma  robot 


i^i 


Fig.  5.3  Elbow  singularity  of  the  Puma  robot 


intersecting  all  three  arm  axes  is  not  as  obvious  and  needs  further  explanation. 
This  line  is  indicated  by  C  in  that  figure,  and  is  parallel  to  the  second  and  third 
axes.  It  is  apparent  that  this  line  intersects  the  first  axis  at  right  angles  at  a  point 
I .  Now,  if  we  take  into  account  that  all  parallel  lines  intersect  at  infinity,  then  it 
becomes  apparent  that  C  intersects  the  axes  of  the  second  and  third  revolutes  as 
well,  and  hence,  C  intersects  all  three  axes. 

Case  2:  Two  consecutive  axes  are  parallel  and  C  lies  in  their  plane,  as  shown 
in  Fig.  5.3.  For  conciseness,  again,  we  assume  that  axes  2  and  3  are  parallel,  a 
rather  common  case  in  industrial  manipulators,  but  the  derivations  below  are  the 
same  if  the  parallel  axes  are  the  first  two.  We  now  let  1123  be  the  plane  of  the  last 
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two  axes  and  1123  its  normal.  Furthermore,  03  =  02  and  r2  =  ri ;  moreover,  the 
intersection  of  O2C  with  £3  is  point  I  of  the  same  figure,  while  O3I  =  d\  then, 
r3  =  ^/03  +  aY2  and  03  x  r3  =  ^(02  x  Y2),  where 


in  terms  of  the  Denavit-Hartenberg  notation,  thereby  making  apparent  that 
the  last  two  columns  of  J21  are  linearly  dependent.  Moreover,  02  x  Y2  and, 
consequently,  03  x  r3  are  parallel  to  023,  the  last  two  axes  being  capable  of 
producing  velocities  of  C  only  in  the  direction  of  023.  Hence,  velocities  of  C 
in  1123  that  are  normal  to  01  x  ri,  i.e.,  along  line  £,  cannot  be  produced  in  this 
configuration,  and  the  manipulator  loses,  again,  to  a  first-order  approximation, 
one  degree  of  freedom.  The  set  of  infeasible  velocities,  then,  is  parallel  to  the 
line  C  of  Fig.  5.3,  whose  direction  is  the  geometric  representation  of  the  null 
space  of  •  The  singularity  displayed  in  the  foregoing  figure,  termed  here  the 
elbow  singularity,  pertains  also  to  a  manipulator  with  the  architecture  of  Fig.  4.3. 
Notice  that  motions  along  C  in  the  posture  displayed  in  Fig.  5.3  are  possible,  but 
only  in  one  direction,  from  C  to  O2. 

With  regard  to  the  wrist  singularities,  these  were  already  studied  when  solving 
the  orientation  problem  for  the  inverse  displacement  analysis  of  decoupled  manip¬ 
ulators.  Here,  we  study  the  same  in  light  of  the  sub- Jacobian  J12  of  Eq.  (5.20b). 
This  sub- Jacobian  obviously  vanishes  when  the  wrist  is  so  configured  that  its  three 
revolute  axes  are  coplanar,  which  thus  leads  to 


04  X  05  •  06  =  0 

Note  that  when  studying  the  orientation  problem  of  decoupled  manipulators,  we 
found  that  orthogonal  wrists  are  singular  when  the  sixth  and  fourth  axes  are  aligned, 
in  full  agreement  with  the  foregoing  condition.  Indeed,  if  these  two  axes  are  aligned, 
then  04  =  —06,  and  the  above  equation  holds. 


5.4.1  Manipulator  Workspace 

The  workspace  of  spherical  wrists  for  orientation  tasks  was  discussed  in  Sect.  4.4.2. 
Here  we  focus  on  the  workspaces  of  three-axis  positioning  manipulators  in  light  of 
their  singularities. 

In  order  to  gain  insight  into  the  problem,  we  study  first  the  workspace  of 
manipulators  with  the  architecture  of  Fig.  4.3.  Figures  5.2  and  5.3  show  such  a 
manipulator  with  point  C  at  the  limit  of  its  positioning  capabilities  in  one  direction, 
i.e.,  at  the  boundary  of  its  workspace.  Moreover,  with  regard  to  the  posture  of 
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Fig.  5.4  Workspace  of  a  Puma  manipulator  (a)  top  view;  (b)  cross-section;  and  (c)  3D  view 


Fig.  5.2,  it  is  apparent  that  the  first  singularity  is  preserved  if  (a)  point  C  moves 
on  a  line  parallel  to  the  first  axis  and  intersecting  the  second  axis;  and  (b)  with  the 
second  and  third  joints  locked,  the  first  joint  goes  through  a  full  turn.  Under  the 
second  motion,  the  line  of  the  first  motion  sweeps  a  circular  cylinder  whose  axis 
is  the  first  manipulator  axis  and  with  radius  equal  to  the  shoulder  offset.  This 
cylinder  constitutes  a  part  of  the  workspace  boundary,  the  other  part  consisting  of  a 
spherical  surface.  Indeed,  the  second  singularity  is  preserved  if  (a)  with  point  C  in 
the  plane  of  the  second  and  third  axes,  the  second  joint  makes  a  full  turn,  thereby 
tracing  a  circle  with  center  on  £2,  the  plane  of  circle  lying  a  distance  from  the 

first  axis,  with  the  circle  radius  r  =  ^2  +  -yj a\  +  and  (b)  with  point  C  still 
in  the  plane  of  the  second  and  third  joints,  the  first  joint  makes  a  full  turn.  Under 
the  second  motion,  the  circle  generated  by  the  first  motion  describes  a  sphere  of 

radius  R  =  -yjb^  +  because  any  point  of  that  circle  lies  a  distance  R  from  the 
intersection  of  the  first  two  axes.  The  point  Oi ,  that  coincides  with  O2,  thus  becomes 
the  center  of  the  sphere,  which  is  the  second  part  of  the  workspace,  as  shown  in 
Fig.  5.4. 

The  determination  of  the  workspace  boundaries  of  more  general  manipulators 
requires,  obviously,  more  general  approaches,  like  that  proposed  by  Ceccarelli 
(1996).  By  means  of  an  alternative  approach,  Ranjbaran  et  al.  (1992)  found 
the  workspace  boundary  with  the  aid  of  the  general  characteristic  equation  of  a 
three-revolute  manipulator.  This  equation  is  a  quartic  polynomial,  as  displayed  in 
Eq.  (4.25).  From  the  discussion  of  Sect.  4.4.1,  it  is  apparent  that  at  singularities,  two 
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distinct  roots  of  the  IDP  merge  into  a  single  one.  This  happens  at  points  where  the 
plot  of  the  characteristic  polynomial  of  Eq.  (4.25)  is  tangent  to  the  13  axis,  which 
occurs  in  turn  at  points  where  the  derivative  of  this  polynomial  with  respect  to  13 
vanishes.  The  condition  for  O3  to  correspond  to  a  point  C  on  the  boundary  of  the 
workspace  is,  then,  that  both  the  characteristic  polynomial  and  its  derivative  with 
respect  to  vanish  concurrently.  These  two  polynomials  are  displayed  below: 

Pizs)  =  Rz^  +  Sz^  +  Tzl  +  C/t3  +  F  =  0  (5.37a) 

P'(t3)  =  ARzl  +  3Szl  +  2Tz3  +  U  =0  (5.37b) 

with  coefficients  R,  S,T,U,  and  V  defined  in  Eqs.  (4.26a-e).  Erom  these  equations 
and  Eqs.  (4.19d-f)  and  (4.20d-f),  it  is  apparent  that  the  foregoing  coefficients 
are  solely  functions  of  the  manipulator  architecture  and  the  Cartesian  coordinates 
of  point  C.  Moreover,  from  the  same  equations,  it  is  apparent  that  the  above 
coefficients  are  all  quadratic  in  and  quartic  in  zc-  Thus,  since 

the  Cartesian  coordinates  xc  and  yc  do  not  appear  in  the  foregoing  coefficients 
explicitly,  the  workspace  is  symmetric  about  the  Zi  axis,  a  result  to  be  expected 
by  virtue  of  the  independence  of  singularities  from  angle  0\ .  Hence,  the  workspace 
boundary  is  given  by  a  function  /(p^,  =  0  that  can  be  derived  by  eliminating 

r3  from  Eqs.  (5.37a  and  b).  This  can  be  readily  done  by  resorting  to  any  elimination 
procedure,  the  simplest  one  being  dialytic  elimination,  as  discussed  below. 

In  order  to  eliminate  13  from  the  above  two  equations,  we  proceed  in  two  steps: 
In  the  first  step,  six  additional  polynomial  equations  are  derived  from  Eqs.  (5.37a 
and  b)  by  multiplying  the  two  sides  of  each  of  these  equations  by  13,  r|,  and  r|, 
thereby  obtaining  a  total  of  eight  polynomial  equations  in  13,  namely, 

Rz]  +  Szl  +  Tzl  +  Uzl  +  Vzl  =  0 
4Rzl  +  3Szl  +  2Tzl  +  Uzl  =  0 
Rz!  +  Szl  +  Tzl  +  Uzl  +  Vzl  =  0 
4Rzl  +  3Szl  +  2Tzl  +  Uzl  =  0 
Rzl  +  Szl  +  Tzl  +  Uzl  +  Vz3  =  0 
4Rzl  +  3Szl  +  2Tzl  +  Uz3=0 
Rzl  +  Szl  +  Tzl  +  Uz3  +  V  =  0 
4Rzl  +  3Szl  +  2Tz3  +  U  =  0 

In  the  second  elimination  step  we  write  the  above  eight  equations  in  linear 
homogeneous  form,  namely. 


MT3  =  0 


(5.38a) 
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Fig.  5.5  The  workspace  of 
the  manipulator  of 
Figs.  4.15-4.17 


Regions  Number  of  Solutions 


with  the  8x8  matrix  M  and  the  eight-dimensional  vector  T3  defined  as 


1 

0 

0 

0 

1 

_ 1 

0  AR?>S  IT  U  0  0  0 

r6 

r5 

0  0  AR?>S  IT  U  0  0 

,  T3  = 

^3 

0  0  0  AR?>S  IT  U  0 

r2 

^3 

0 

0 

0 

^3 

_0  0  0  0  AR?>S  IT  U_ 

_  1  _ 

(5.38b) 


It  is  now  apparent  that  any  feasible  solution  of  Eq.  (5.38a)  must  be  nontrivial,  and 
hence,  M  must  be  singular.  The  desired  boundary  equation  is  then  derived  from  the 
singularity  condition  on  M,  i.e., 

/(p2,  zc)  =  det(M)  =  0  (5.39) 

As  a  matter  of  fact,  function  /(p^,  zc)  of  Eq.  (5.39),  known  as  the  (polynomial) 
resolvent  of  Eqs.  (5.37a  and  b),  can  be  computed  using  computer  algebra,  upon 
invoking  the  procedure  to  obtain  the  discriminant  of  Eq.  (5.37a).^ 

We  used  the  foregoing  procedure,  with  the  help  of  computer  algebra,  to  obtain 
a  rendering  of  the  workspace  boundary  of  the  manipulator  of  Eigs.  4.15-4.17,  the 
workspace  thus  obtained  being  displayed  in  Eig.  5.5.  Eor  the  record,  the  resolvent 


^Although  a  quartic  polynomial  has,  properly  speaking,  four  discriminants  (Yang  et  al.  1996), 
which  are  defined  as  the  central  minors  of  matrix  M  of  Eq.  (5.39)  when  Pix^)  is  written  in 
monic  form — with  leading  coefficient  equal  to  unity — the  resolvent  is  sometimes  referred  to  as 
the  discriminant  in  question. 
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of  this  manipulator,  given  in  Eq.  (5.39),  turned  out  to  be  a  16th-degree  bivariate 
polynomial  in  p  and  zc^  involving  only  even  powers.  That  is,  the  resolvent  in 
question  turns  out  to  be  a  bivariate  octic  polynomial  in  p^  and  .  If  we  let  a  =  p^ 
and  ^  then 

/(a,0  =  ^*+(-2+CT)^’+(7a^-32CT+27)^®+(7a^-54CT^+97CT-42)^^ 

+  (35a'‘-400CT^  +  1210a^-976a+283)^'‘+(7CT^-110a'‘+510CT^ 
-684a2  +  123CT-70)^^+(7a'’-144a®+965a'^-2208CT^+705CT2 
-272  a+83)^2+8  ct-1)(ct^-22  154  ct^-328 

-155a+14)^+(a'‘-24a^  +  190a^-552a  +  17)(a2-4a  -  1)^=0 


5.5  Acceleration  Analysis  of  Serial  Manipulators 

•  • 

The  subject  of  this  section  is  the  computation  of  vector  0  of  second  joint- 
variable  derivatives,  also  called  the  joint  accelerations.  This  vector  is  computed 
from  Cartesian  position,  velocity,  and  acceleration  data.  To  this  end,  both  sides  of 
Eq.  (5.9)  are  differentiated  with  respect  to  time,  thus  obtaining 

(5.40) 


and  hence. 


^=J“‘(t-j^)  (5.41) 

Erom  Eq.  (5.40),  it  is  apparent  that  the  joint- acceleration  vector  is  computed  in 
exactly  the  same  way  as  the  joint-rate  vector.  In  fact,  the  LU  decomposition  of  J  is 
the  same  in  this  case  and  hence,  need  not  be  recomputed.  All  that  is  needed  is  the 
solution  of  a  lower-  and  an  upper-triangular  system,  namely, 

Lz  =  t  —  j^,  =  z 


•  • 

The  two  foregoing  systems  are  solved  first  for  z  and  then  for  0  by  forward  and 
backward  substitution,  respectively.  The  first  of  the  foregoing  systems  is  solved  with 
Ml!  multiplications  and  A'j  additions;  the  second  with  M^"  multiplications  and  A'j' 
additions.  These  figures  appear  in  Eqs.  (5.12b  and  c).  Thus,  the  total  numbers  of 
multiplications  Mt  and  additions  At  that  the  forward  and  backward  solutions  of  the 
aforementioned  systems  require  are 


Mf  =  n^,  At  =  n(n  —  1) 


(5.42) 
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In  Eq.  (5.40),  the  right-hand  side  comprises  two  terms,  the  first  being  the  specified 
time-rate  of  change  of  the  twist  of  the  EE,  or  twist-rate,  for  brevity,  which  is 
readily  available.  The  second  term  is  not  available  and  must  be  computed.  This 
term  involves  the  product  of  the  time-derivative  of  J  times  the  previously  computed 
joint-rate  vector.  Hence,  in  order  to  evaluate  the  right-hand  side  of  that  equation,  all 
that  is  further  required  is  J.  Erom  Eq.  (5.10a),  one  has 


where,  from  Eqs.  (5.7a  and  b), 

A  =  [eie2---e„]  (5.43a) 

B  =  [lii  U2  (5.43b) 

and  U;  denotes  x  r/ ,  for  /  =  1 , 2, . . . ,  /7.  Moreover, 

01  =  0)0  X  01  =  0  (5.44a) 

0;  =  X  0;  =  0),  X  0; ,  i  =  2,3, ...  ,n  (5.44b) 

and 

li,-  =  e,-  X  r,-  +  e,-  X  f^- ,  i  =  1,2, . . .  ,n  (5.44c) 


Next,  an  expression  for  f/  is  derived  by  time-differentiating  both  sides  of  Eq.  (5.6), 
which  produces 


Vi  =  ki  +  a/+i  H - h  a„,  i  =  1,2,...  n 

Recalling  Eq.  (5.4),  the  above  equation  reduces  to 

ici  =  (Oi  X  a,  +  0)^+1  X  a^+i  H - h  x  a„  (5.45) 

Substitution  of  Eqs.  (5.44)  and  (5.45)  into  Eqs.  (5.43a  and  b)  leads  to 
A  =  [o  0)1  X  02  •••  (On-I  X  e„] 

B  =  [ei  X  fi  0)12  X  r2  +  62  X  1*2  •  •  •  (On-i,n  X  r„  +  e„  X  f„] 


with  Vk  and  (Ok,k-\-i  defined  as 


n 

h  xai,  k  = 

k 


—  ^k  X  +  k  —  \,  .  .  .  ,fl  1 


(5.46a) 


(5.46b) 
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The  foregoing  expressions  are  invariant  and  hence,  valid  in  any  coordinate  frame. 
However,  these  expressions  have  to  be  incorporated  into  matrix  J;  then,  the  latter 

is  to  be  multiplied  by  vector  0,  as  indicated  in  Eq.  (5.40).  Thus,  eventually  all 

•  • 

columns  of  both  A  and  B  will  have  to  be  represented  in  the  same  coordinate  frame. 
Hence,  coordinate  transformations  will  have  to  be  introduced  in  the  foregoing  matrix 
columns  in  order  to  have  all  of  these  represented  in  the  same  coordinate  frame,  say, 
the  first  one.  We  then  have  the  expansion  below: 


II 

o’ 

• 

+  02 

'^2' 

• 

+  ■  ■  •  + 

• 

_U1_ 

_U2_ 

Un_ 

(5.47) 


The  right-hand  side  of  Eq.  (5.47)  is  computed  recursively  as  described  below  in  five 
steps,  the  number  of  operations  required  being  included  at  the  end  of  each  step. 

1 .  Compute  {[o)i]i  : 

For  i  =  1  to  n  —  1  do 


enddo 

2.  Compute  { [e,  ], 

[ei]i 

For  i 


[0]i 
2  to 


n  do 


8(^  -l)M  &5(n-l)  A 


enddo  OM  &  OA 

3.  Compute  {  [r,  ]/}'[: 

[  ]n  ^  [^n  ^  ^n]n 

For  i  =  n  —  1  to  1  do 


[r/],-  ^  [w,- X  a,- ],- +  Q,  [r/+i],+i 


enddo  (14/7  —  8)M  &  (10/7  —  7)^4 

4.  Compute  {  [u,  ]/  using  the  expression  appearing  in  Eq.  (5.44c): 

[ui]i  ^[eixriji  For  i  =  2  to  n  do 

[li,],-  ^  [e,-  xr,-  -he,-  x  r,- ],- 

enddo  4(n  —  1)  M  &  3(n  —  1)  A 

•  • 

5.  Compute!^: 

•  • 

Let  V  =  J^,  which  is  a  six-dimensional  vector.  A  coordinate  transformation  of 
its  two  three-dimensional  vector  components  will  be  implemented  using  the  6x6 
matrices  U/ ,  which  are  defined  as 
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Q,  O 
O  Q, 


where  O  stands  for  the  3x3  zero  matrix.  Thus,  the  foregoing  6x6  matrices  are 
block-diagonal,  their  diagonal  blocks  being  simply  the  matrices  Q/  introduced  in 
Sect.  4.2.  One  then  has  the  algorithm  below: 


[v] 


n 


e. 


n 


i_  -in 

For  i  =  n  —  1  to  1  do 


+  U/  [  v]/+i 


enddo 

iO  ^  [v]i  21(^-l)  +  4M&13(^-l)v4 

•  • 

thereby  completing  the  computation  of  . 

The  figures  given  above  for  the  floating-point  operations  involved  were  obtained 
based  on  a  few  facts,  namely, 

1.  It  is  recalled  that  [C;  ]/  =  [0,  0,  1  ]^.  Moreover,  if  we  let  [w]/  =  [wx,  Wy, 
be  an  arbitrary  three-dimensional  vector,  then 


[e/  X  w],- 


Wx 

0 


this  product  thus  requiring  zero  multiplications  and  zero  additions. 

2.  [e,  ]i,  computed  as  in  Eq.  (5.44b),  takes  on  the  form  [cOy,  —cOx^  0]^^  where  cOx 
and  cOy  are  the  Xt  and  Yi  components  of  0)^  .  Moreover,  let  [r,  ];  =  [x,  j,  z]^. 
Then 


[e/  X  r,-  ],- 


xcox  +  yooy 


and  this  product  is  computed  with  four  multiplications  and  one  addition. 

3.  As  found  in  Sect.  5.3,  any  coordinate  transformation  from  J^i  to  J^+i,  or  vice 
versa,  of  any  three-dimensional  vector  is  computed  with  eight  multiplications 
and  four  additions. 

•  • 

Thus,  the  total  numbers  of  multiplications  and  additions  required  to  compute  iO 
in  frame  denoted  by  M/  and  A /,  respectively,  are  as  shown  below: 


Mj  =  47n  -  37,  Aj  =  3ln  -  28 
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Since  the  right-hand  side  of  Eq.  (5.40)  involves  the  algebraic  sum  of  two 
six-dimensional  vectors,  then,  the  total  numbers  of  multiplications  and  additions 
needed  to  compute  the  aforementioned  right-hand  side,  denoted  by  Mr  and  are 

Mr  =  Ain  —  37,  Ar  =31/7—22 

These  figures  yield  245  multiplications  and  164  additions  for  a  six-revolute  manip¬ 
ulator  of  arbitrary  architecture.  Finally,  if  the  latter  figures  are  added  to  those  of 
Eq.  (5.42),  one  obtains  the  numbers  of  multiplications  and  additions  required  for  an 
acceleration  resolution  of  a  six-revolute  manipulator  of  arbitrary  architecture  as 

Ma  =281,  ^  =  194 

Furthermore,  for  six-axis,  decoupled  manipulators,  the  operation  counts  of 
steps  1  and  2  above  do  not  change.  However,  step  3  is  reduced  by  42  multiplications 
and  30  additions,  whereas  step  4  by  12  multiplications  and  9  additions.  Moreover, 

step  5  is  reduced  by  63  multiplications  and  39  additions.  With  regard  to  the  solution 

•  • 

of  Eq.  (5.40)  for  0,  an  additional  reduction  of  floating-point  operations,  or  flops,  is 
obtained,  for  now  we  need  only  18  multiplications  and  12  additions  to  solve  two 
systems  of  three  equations  with  three  unknowns,  thereby  saving  18  multiplications 
and  18  additions.  Thus,  the  corresponding  figures  for  such  a  manipulator,  M'^  and 
A'^ ,  respectively,  are 


M’,  =  U6,  4=98 


5.6  Static  Analysis  of  Serial  Manipulators 

In  this  section,  the  static  analysis  of  a  serial  /7-axis  manipulator  is  undertaken, 
six-axis  decoupled  manipulators  being  treated  as  special  cases.  Let  Xi  be  either 
the  torque  acting  at  the  /th  revolute  or  the  force  acting  at  the  /th  prismatic  pair. 
Moreover,  let  t  be  the  /7 -dimensional  vector  of  joint  forces  and  torques,  whose 
i  th  component  is  r, ,  whereas  w  =  [  ^  denotes  the  wrench  exerted  by  the 

environment  on  the  EE,  with  n  denoting  the  resultant  moment  and  f  the  resultant 
force  applied  at  point  P  of  the  end-effector  of  the  manipulator  of  Fig.  5.1.  Then,  the 
power  exerted  on  the  manipulator  by  all  forces  and  moments  acting  on  the  EE  is 

=  w^t  =  (o  +  f^p 

whereas  the  power  Tt /  exerted  on  the  manipulator  by  all  joint  motors  is 

Uj  =r^0 


(5.48) 
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Under  static,  conservative  conditions,  there  is  neither  power  dissipation  nor 
change  in  the  kinetic  energy  of  the  manipulator,  and  hence,  the  two  foregoing 
powers  are  equal,  which  is  just  a  restatement  of  the  First  Law  of  Thermodynamics 
or,  equivalently,  a  form  of  the  Principle  of  Virtual  Work,  i.e., 

w^t  =  r^O  (5.49a) 

Upon  substitution  of  Eq.  (5.9)  into  Eq.  (5.49a),  we  obtain 

=  t'^0  (5.49b) 

which  is  a  relation  valid  for  arbitrary  0 .  Under  these  conditions,  if  J  is  not  singular, 
Eq.  (5.49b)  leads  to 


J 


T 


W  = 


T 


(5.50) 


This  equation  relates  the  wrench  acting  on  the  EE  with  the  joint  forces  and  torques 
exerted  by  the  actuators.  Therefore,  this  equation  finds  applications  in  the  sensing 
of  the  wrench  w  acting  on  the  EE  by  means  of  torque  sensors  located  at  the  revolute 
axes.  These  sensors  measure  the  motor- supplied  torques  via  the  current  flowing 
through  the  motor  armatures,  the  sensor  readouts  being  the  joint  torques — or  forces, 
in  the  case  of  prismatic  joints-  -{n  fx,  grouped  in  vector  t  . 

Eor  a  six-axis  manipulator,  in  the  absence  of  singularities,  the  foregoing  equation 
can  be  readily  solved  for  w  in  the  form 

W  =  J-^T 


where  stands  for  the  inverse  of  J^.  Thus,  using  the  figures  recorded  in 
Eq.  (5.18b),  w  can  be  computed  from  Eq.  (5.50)  with  127  multiplications  and  100 
additions  for  a  manipulator  of  arbitrary  architecture.  However,  if  the  manipulator  is 
of  the  decoupled  type,  the  Jacobian  takes  on  the  form  appearing  in  Eq.  (5.19),  and 
hence,  the  foregoing  computation  can  be  performed  in  two  steps,  namely. 


J 


T 


r 


w 


w 


where  is  the  resultant  moment  acting  on  the  end-effector  when  f  is  applied  at  the 
center  of  the  wrist,  while  r  has  been  partitioned  as 


with  r a  and  defined  as  the  wrist  and  the  arm  torques,  respectively.  These  two 
vectors  are  given,  in  turn,  as 


"u" 

~u~ 

Ta  = 

,  T  - 

_U_ 

_^6_ 
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Hence,  the  foregoing  calculations,  as  pertaining  to  a  six-axis,  decoupled  manipula¬ 
tor,  are  performed  with  55  multiplications  and  37  additions,  which  follows  from  a 
result  that  was  derived  in  Sect.  5.2  and  is  summarized  in  Eq.  (5.23). 

In  solving  for  the  wrench  acting  on  the  EE  from  the  above  relations,  the  wrist 
equilibrium  equation  is  first  solved  for  n^,  thus  obtaining 

=  (5.51) 

where  stands  for  the  inverse  of  J^2’  available  in  Eq.  (5.29).  Therefore, 

^  [(es  X  es)  (e^  x  e4)  (e4  x  ej)] 

^12 

=  ^[14(05  X  05)  +  15(05  X  04)  +  15(04  X  05)  ]  (5.52) 

^12 

Now,  if  we  let 

Ta  =  a  (5.53) 

we  have,  from  Eq.  (5.28), 


f  =  [U2 


X  U3  U3  X 


Ui  Ui  X  U2] 


^  a 

A21 


where 


u,-  =  0/  X  r,- 


or 


f  =  — [ri(u2  X  U3)  +  r2(u3  X  ui)  +  r3(ui  x  U2)]  (5.54) 

^21 


5.7  Planar  Manipulators 

Shown  in  Eig.  5.6  is  a  three-axis  planar  manipulator.  Note  that  in  this  case,  the  DH 
parameters  bi  and  ai  vanish,  for  f  =  1,2,3,  the  nonvanishing  parameters  at  being 
indicated  in  the  same  figure.  Below  we  proceed  with  the  displacement,  velocity, 
acceleration,  and  static  analyses  of  this  manipulator.  Here,  we  recall  a  few  relations 
of  planar  mechanics  that  will  be  found  useful  in  the  discussion  below. 

A  2  X  2  matrix  A  can  be  partitioned  either  columnwise  or  rowwise,  as  shown 
below: 


A  =  [a  b] 
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Fig.  5.6  Three-axis  planar  manipulator 


where  a,  b,  c,  and  d  are  all  two-dimensional  column  vectors.  Furthermore,  let  E  be 
defined  as  an  orthogonal  matrix  rotating  two-dimensional  vectors  through  an  angle 
of  90°  counterclockwise.  Hence, 


0  -1 
1  0 


(5.55) 


We  thus  have 

Fact  5.7.1. 


and  hence. 

Fact  5.7.2. 


=  -E 


where  1  is  the  2x2  identity  matrix.  Moreover, 

Fact  5.7.3. 

det(A)  =  -a^Eb  =  b^Ea  =  -c^Ed  =  d^Ec 

and 
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Fact  5.7.4. 


A-i  = 


1 


J 


det(A) 


E  = 


det(A) 


E  [-d  c] 


5. 7. 1  Displacement  Analysis 

The  inverse  displacement  analysis  of  the  manipulator  at  hand  now  consists  in 
determining  the  values  of  angles  0/,  for  /  =  1,2,3,  that  will  place  the  end- 
effector  so  that  its  operation  point  P  will  be  positioned  at  the  prescribed  Cartesian 
coordinates  x,  y  and  be  oriented  at  a  given  angle  0  with  the  X  axis  of  Fig.  5.6. 
Note  that  this  manipulator  can  be  considered  as  decoupled,  for  the  end-effector  can 
be  placed  at  the  desired  pose  by  first  positioning  point  O3  with  the  aid  of  the  first 
two  joints  and  then  orienting  it  with  the  third  joint  only.  We  then  solve  for  the  joint 
angles  in  two  steps,  one  for  positioning  and  one  for  orienting. 

We  now  have,  from  the  geometry  of  Fig.  5.6, 

aici  +  a2Ci2  =  X 

aisi  +  a2Si2  =  y 

where  x  and  y  denote  the  Cartesian  coordinates  of  point  O3,  while  cu  and  ^12 

stand  for  cos(^i  +  O2)  and  sin(^i  +  62),  respectively.  We  have  thus  derived  two 

equations  for  the  two  unknown  angles,  from  which  we  can  determine  these  angles 
in  various  ways.  For  example,  we  can  solve  the  problem  using  a  semigraphical 
approach  similar  to  that  of  Sect.  9.4. 

Indeed,  from  the  two  foregoing  equations  we  can  eliminate  both  C12  and  ^12  by 
solving  for  the  second  terms  of  the  left-hand  sides  of  those  equations,  namely, 

a2Ci2  =  X  —  aiCi  (5.56a) 

^2^12  =  y  —  ciiSi  (5.56b) 


If  both  sides  of  the  above  two  equations  are  now  squared,  then  added,  and  the 
ensuing  sum  is  equated  to  we  obtain,  after  simplification,  a  linear  equation  in  Ci 
and  that  represents  a  line  C  in  the  ci-^i  plane: 

jC:  —a\  a\-\-  laixci  -h  2aiysi  —  (x^  +  y^)  =  0  (5.57) 

Apparently,  the  two  foregoing  variables  are  constrained  by  a  quadratic  equation 
defining  a  circle  C  in  the  same  plane: 


C: 


c\^-  s\  =  \ 
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Fig.  5.7  The  two  real  solutions  of  a  planar  manipulator 


Fig.  5.8  The  two  real  values 
of  0\  giving  the  two  postures 
depicted  in  Fig.  5.7 


which  has  a  unit  radius  and  is  centered  at  the  origin  of  the  plane.  The  real 
roots  of  interest  are  then  obtained  as  the  intersections  of  C  and  C.  Thus,  the  problem 
can  admit  (a)  two  real  and  distinct  roots,  if  the  line  and  the  circle  intersect;  (b)  one 
repeated  root  if  the  line  is  tangent  to  the  circle;  and  (c)  no  real  root  if  the  line  does 
not  intersect  the  circle  (Fig.  5.7). 

With  Cl  and  si  known,  angle  0i  is  fully  determined.  Note  that  the  two  real 
intersections  of  C  with  C  provide  each  one  value  of  0\,  as  depicted  in  Fig.  5.8. 

Once  0i  and  O2  are  available,  0^  is  readily  derived  from  the  geometry  of  Fig.  5.6, 
namely. 


^3  —  0  “  (^1  +  ^2) 


and  hence,  each  pair  of  (0i ,  O2)  values  yields  one  single  value  for  O2 .  Since  we  have 
two  such  pairs,  the  problem  admits  two  real  solutions. 
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5. 7.2  Velocity  Analysis 

Velocity  analysis  is  most  easily  accomplished  if  the  general  velocity  relations 
derived  in  Sect.  5.2  are  recalled  and  adapted  to  planar  manipulators.  Thus  we  have, 
as  in  Eq.  (5.9), 

JO  =  t  (5.58a) 


where  now. 


ei  02  03 


01  X  ri  02  X  r2  03  X  r3 


9 


(5.58b) 


and  {  Vi  are  dehned  as  in  Eq.  (5.6),  i.e.,  as  the  vectors  directed  from  Oi  to  T*.  As 
in  the  previous  subsection,  we  assume  here  that  the  manipulator  moves  in  the  X-Y 
plane,  and  hence,  ah  revolute  axes  are  parallel  to  the  Z  axis,  vectors  0,  and  ,  for 
i  =  1,2,3,  thus  taking  on  the  forms 


"O' 

01  =  02  =  03  =  0  = 

0 

,  Yi  = 

yi 

_1_ 

_o_ 

with  t  reducing  to 

t=[00  ^  Xp  yp  of  (5.58c) 

in  which  xp  and  yp  denote  the  components  of  the  velocity  of  P.  Thus, 


e,-  X  Vi 


-yi 

0 


and  hence,  the  foregoing  cross  product  can  be  expressed  as 


e,-  X  Yi 


Es,- 

0 


where  E  was  dehned  in  Eq.  (5.55)  and  S/  is  the  two-dimensional  projection  of  r/ 

T 

onto  the  X-Y  plane  of  motion,  i.e.,  s,  =  [x,  j,  ]  .  Equation  (5.58a)  thus  reduces  to 


"  0 

0 

0  ■ 

"O' 

1 

1 

1 

• 

0  = 

0 

Esi 

ES2 

ES3 

P 

_  0 

0 

0  _ 

_0_ 

(5.59) 
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where  0  is  the  two-dimensional  zero  vector  and  p  is  now  reduced  to  p  =  [i,  yY . 
In  summary,  then,  by  working  only  with  the  three  nontrivial  equations  of  Eq.  (5.59), 
we  can  represent  the  velocity  relation  using  a  3  x  3  Jacobian  in  Eq.  (5.58a).  To  this 
end,  we  redefine  J  and  t  as 


111" 

t  = 

'f 

ESi  ES2  ES3_ 

,  1  — 

LpJ 

(5.60) 


The  velocity  resolution  of  this  manipulator  thus  reduces  to  solving  for  the  three  joint 
rates  from  Eq.  (5.58a),  with  J  and  t  defined  as  in  Eq.  (5.60),  which  thus  leads  to  the 
system  below: 


1  1  1 
ESi  ES2  ES3 


~0i~ 

Oi 

A. 


(5.61) 


Solving  for  {  4  }i  is  readily  done  by  first  reducing  the  system  of  equations 
appearing  in  Eq.  (5.58a)  to  one  of  two  equations  in  two  unknowns  by  resorting  to 
Gaussian  elimination.  Indeed,  if  the  first  scalar  equation  of  Eq.  (5.61)  is  multiplied 
by  Esi  and  the  product  is  subtracted  from  the  two-dimensional  vector  equation,  we 
obtain 


1  1  1 
0  E(s2  -Si)  E(s3  -  Si) 


~0i~ 

O2 

A. 


(5.62) 


from  which  a  reduced  system  of  two  equations  in  two  unknowns  is  readily  obtained, 
namely. 


[E(S2  -  Si)  E(S3  -  Si)] 


p  —  0Esi 


(5.63) 


The  system  of  equations  (5.63)  can  be  readily  solved  if  Eact  5.7.4  is  recalled, 
namely. 


'ei 

1 

■-(S3-Si)^E' 

A. 

“  A 

.  (S2-Si)^E  _ 

E(p  —  0Esi) 


y  r  (S3  -si)^(p-(^Esi) 

A  [-(S2  -Si)^(p-0Esi) 


where  A  is  the  determinant  of  the  2x2  matrix  coefficient  of  Eq.  (5.63),  i.e., 

A  =  det([E(s2  -  si)  E(s3  -  si)])  =  -(S2  -  Si)^E(s3  -  Si)  (5.64) 
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We  thus  have 


(S3  -Si)^(p-^ESi) 
(S2  -  Si)^E(S3  -  Si) 

(S2-Si)^(p-0ESi) 
(S2  -Si)^E(S3  -  Si) 


(5.65a) 


(5.65b) 


Further,  0i  is  computed  from  the  first  scalar  equation  of  Eq.  (5.61),  i.e.. 


0l  —  0  —  (^2  +  ^3) 


(5.65c) 


thereby  completing  the  velocity  analysis. 

The  foregoing  calculations  are  summarized  below  in  algorithmic  form,  with  the 
corresponding  numbers  of  multiplications  and  additions  indicated  at  each  stage. 
In  those  numbers,  we  have  taken  into  account  that  a  multiplication  of  E  by  any  two- 
dimensional  vector  incurs  no  computational  cost,  but  rather  a  simple  rearrangement 
of  the  entries  of  this  vector,  with  a  reversal  of  one  sign. 


1.  d2l  ^  S2  -  Si 

2.  d3i  ^83  —  81 

3.  A  —  d^j^Ed2i 

4.  u  ^  p  —  (pEsi 

5.  u  ^  u/A 

6.  02  ^  U^d3i 

7.  02  ^  -U^d21 

8.  di  < —  0  —  62  —  O2 


OM  +  2v4 
OM  +  2v4 
2M  +  lv4 
2M  ^2A 
2M  +0^ 
2M  +  lv4 
2M  +  lv4 
OM  +  2.4 


The  complete  calculation  of  joint  rates  thus  consumes  only  lOM  and  11+, 
which  represents  a  savings  of  about  67%  of  the  computations  involved  if  Gaussian 
elimination  is  applied  without  regarding  the  algebraic  structure  of  the  Jacobian  J  and 
its  kinematic  and  geometric  significance.  In  fact,  the  solution  of  an  arbitrary  system 
of  three  equations  in  three  unknowns  requires,  from  Eq.  (5.18a),  28  additions  and 
23  multiplications.  If  the  cost  of  calculating  the  right-hand  side  is  added,  namely, 
A  A  and  6M,  a  total  of  32+  and  29  M  is  required  to  solve  for  the  joint  rates  if 
straightforward  Gaussian  elimination  is  used. 


5. 7. 3  Acceleration  Analysis 

The  calculation  of  the  joint  accelerations  needed  to  produce  a  given  twist  rate  of 
the  EE  is  readily  accomplished  by  differentiating  both  sides  of  Eq.  (5.58a),  with 
definitions  (5.60),  i.e.. 


JO  ^  jo  =i 
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•  • 

from  which  we  readily  derive  a  system  of  equations  similar  to  Eq.  (5.58a)  with  0  as 
unknown,  namely, 

•  •  •  •  • 

30  =i-30 

where 


1 

1 _ 

_  •  • 

• 

J  = 

■  0  0  o' 

•  • 

,  0  = 

,  t  = 

0 

_ESi  ES2  ES3_ 

•  • 

_P_ 

S3  —  (^1  +  ^2  +  ^3)Ea3 

•  • 

S2  —  ^2  +  S3  =  (^1  +  02)Ea2  +  S3 
Si  =  ai  +  S2  =  ^lEai  +  S2 


Now  we  can  proceed  by  Gaussian  elimination  to  solve  for  the  joint  accelerations 
in  exactly  the  same  manner  as  in  Sect.  5.7.2,  thereby  obtaining  the  counterpart  of 
Eq.  (5.63),  namely. 


[E(S2  -  Si)  E(S3  -  Si)]  2  =  W 

(5.66a) 

with  w  defined  as 

W  =  p  -  E(0iSi  +  02S2  +  03S3  +  ^Si) 

(5.66b) 

and  hence,  similar  to  Eqs.  (5.65a-c),  one  has 

2  (S3  -  Si)^W 

^2  — 

A 

(5.67a) 

••  (S2  -  Si)^W 

(73  = 

A 

(5.67b) 

0*1  =  0  —  (02  +  03) 

(5.67c) 

Below  we  summarize  the  foregoing  calculations  in  algorithmic  form,  indicating 
the  numbers  of  operations  required  at  each  stage. 


1.  S3  ^  (^1  +  ^2  +  ^3)Ea3 

2.  S2  ^  (^1  +  02)Ea2  +  S3 

3.  Si  ^  ^lEai  +  S2 

4.  w  ^  p  -  E(diSi  +  4s2  +  O3S3  +  0Si) 

5.  w  ^  w/A 

•  • 

6.  O2  ^  w^d3i 

7.  O3  < - w^d2i 

8.  01  ^  0  -  (02  +  03) 


2M  Sc  2 A 
2M  Sc  3  A 
2M  Sc  2 A 
8M  &8v4 
2M  +0^ 
2M  +  lv4 
2M  +  lv4 
OM  +  2v4 
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where  d2i,  d3i,  and  A  are  available  from  velocity  calculations.  The  joint  accel¬ 
erations  thus  require  a  total  of  20  multiplications  and  19  additions.  These  figures 
represent  substantial  savings  when  compared  with  the  numbers  of  operations 
required  if  plain  Gaussian  elimination  were  used,  namely,  33  multiplications  and 
35  additions. 

It  is  noteworthy  that  in  the  foregoing  algorithm,  we  have  replaced  neither  the  sum 

•  •  •  • 

+  ^2  +  ^3  nor  0iE(si  +  S2  +  S3)  by  co  and  correspondingly,  by  p,  because  in  path 
tracking,  there  is  no  perfect  match  between  joint  and  Cartesian  variables.  In  fact, 
joint-rate  and  joint- acceleration  calculations  are  needed  in  feedback  control  schemes 
to  estimate  the  position,  velocity,  and  acceleration  errors  by  proper  corrective 
actions. 


5.7.4  Static  Analysis 


Here  we  assume  that  the  environment  exerts  a  planar  wrench  on  the  EE  of  the 
manipulator  appearing  in  Eig.  5.6.  In  accordance  with  the  definition  of  the  planar 
twist  in  Sect.  5.7.2,  Eq.  (5.60),  the  planar  wrench  is  now  defined  as 


w  = 


(5.68) 


where  n  is  the  scalar  couple  acting  on  the  EE  and  f  is  the  two-dimensional  force 
acting  at  the  operation  point  P  of  the  EE.  If  additionally,  we  denote  by  t  the  three- 
dimensional  vector  of  joint  torques,  the  planar  counterpart  of  Eq.  (5.50)  follows,  i.e., 

J^w  =  T  (5.69) 


where 


J 


T 


(Esi)^ 
(ES2) 
(ES3) 


Now,  in  order  to  solve  for  the  wrench  w  acting  on  the  end-effector,  given  the 
joint  torques  t  and  the  posture  of  the  manipulator,  we  can  still  apply  our  compact 
Gaussian-elimination  scheme,  as  introduced  in  Sect.  5.7.2.  To  this  end,  we  subtract 
the  first  scalar  equation  from  the  second  and  the  third  scalar  equations  of  Eq.  (5.69), 
which  renders  the  foregoing  system  in  the  form 


”1  (Esi)^  ■ 

n 

f 

ri 

0  [E(S2  -  Si)]^ 

— 

X2  -  Tl 

_0  [E(S3  -  Si)]^_ 

1 

1 

1 

m 

_ 1 
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Thus,  the  last  two  equations  have  been  decoupled  from  the  first  one,  which  allows 
us  to  solve  them  separately,  i.e.,  we  have  reduced  the  system  to  one  of  two  equations 
in  two  unknowns,  namely. 


[E(S2-Si)]^ 
[E(S3  -Si)]^ 


T2  -  Ti 
r3  -  Ti 


from  which  we  readily  obtain 


■[E(S2-Si)]^' 

-1 

1 

1 

JE(S3  -Si)]^_ 

(5.70) 


(5.71) 


and  hence,  upon  expansion  of  the  above  inverse, 

f  =  ^  [(t^i  -  ri)(s3  -  Si)  -  (xs  -  ri)(s2  -  Si)]  (5.72) 

where  A  is  exactly  as  defined  in  Eq.  (5.64).  Finally,  the  resultant  moment  n  acting 
on  the  end-effector  is  readily  calculated  from  the  first  scalar  equation  of  Eq.  (5.69), 
namely,  as 


=  ri  +  sf  Ef 

thereby  completing  the  static  analysis  of  the  manipulator  under  study.  A  quick 
analysis  of  computational  costs  shows  that  the  foregoing  solution  needs  8M  and 
6y4,  or  a  savings  of  about  70%  if  straightforward  Gaussian  elimination  is  applied. 


5.8  Kinetostatic  Performance  Indices 

Chapters  6  and  7  do  not  depend  on  this  section,  which  can  thus  be  skipped  in  an 
introductory  course  based  on  the  first  half  of  the  book.  We  have  included  this  section 
because  (a)  it  is  a  simple  matter  to  render  the  section  self-contained,  while  introduc¬ 
ing  the  concept  of  condition  number  and  its  relevance  in  robotics;  (b)  kinetostatic 
performance  can  be  studied  with  the  background  of  the  material  included  up  to 
this  section;  and  (c)  kinetostatic  performance  is  becoming  increasingly  relevant  as  a 
design  criterion  and  as  a  figure  of  merit  in  robot  control. 

A  kinetostatic  performance  index  of  a  robotic  mechanical  system  is  a  scalar 
quantity  that  measures  how  well  the  system  behaves  with  regard  to  force  and  motion 
transmission,  the  latter  being  understood  in  the  differential  sense,  i.e.,  at  the  velocity 
level.  Now,  a  kinetostatic  performance  index,  or  kinetostatic  index  for  brevity,  may 
be  needed  to  assess  the  performance  of  a  robot  at  the  design  stage,  in  which  case 
we  need  a  posture-independent  index.  In  this  case,  the  index  becomes  a  function  of 
the  robot  architecture  only.  If,  on  the  other  hand,  we  want  to  assess  the  performance 
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of  a  given  robot  while  performing  a  task,  what  we  need  is  a  posture-dependent 
index.  This  difference  is  often  overlooked  in  the  robotics  literature,  although  it  is 
extremely  important.  Moreover,  while  performance  indices  can  be  defined  for  all 
kinds  of  robotic  mechanical  systems,  we  focus  here  on  those  associated  with  serial 
manipulators,  which  are  the  ones  studied  most  intensively. 

Among  the  various  performance  indices  that  have  been  proposed,  one  can  cite 
the  concept  of  service  angle,  first  introduced  by  Vinogradov  et  al.  (1971),  and 
the  conditioning  of  robotic  manipulators,  as  proposed  by  Yang  and  Lai  (1985). 
Yoshikawa  (1985),  in  turn,  introduced  the  concept  of  manipulability,  which  is 
defined  as  the  square  root  of  the  determinant  of  the  product  of  the  manipulator 
Jacobian  by  its  transpose.  Paul  and  Stevenson  (1983)  used  the  absolute  value  of 
the  determinant  of  the  Jacobian  to  assess  the  kinematic  performance  of  spherical 
wrists.  Note  that  Yoshikawa’ s  manipulability  is  identical  to  the  absolute  value  of 
the  determinant  of  the  Jacobian,  and  hence,  the  latter  coincides  with  Paul  and 
Stevenson’s  performance  index.  It  should  be  pointed  out  that  these  indices  were 
defined  for  control  purposes  and  hence,  are  posture-dependent.  Germane  to  these 
concepts  is  that  of  dextrous  workspace,  introduced  by  Kumar  and  Waldron  (1981), 
and  used  for  geometric  optimization  by  Vijaykumar  et  al.  (1986).  Although  the 
concepts  of  service  angle  and  manipulability  are  apparently  different,  they  touch 
upon  a  common  underlying  issue,  namely,  the  kinematic,  or  alternatively,  the  static 
performance  of  a  manipulator  from  an  accuracy  viewpoint.  For  this  reason,  we  refer 
to  these  indices  generically  as  kinetostatic. 

What  is  at  stake  when  discussing  the  manipulability  of  a  robotic  manipulator  is  a 
measure  of  the  invertibility  of  the  associated  Jacobian  matrix,  since  this  is  required 
for  velocity  and  force-feedback  control.  One  further  performance  index  is  based  on 
the  condition  number  of  the  Jacobian,  which  was  first  used  by  Salisbury  and  Craig 
(1982)  to  design  mechanical  fingers.  Here,  we  shall  use  this  concept  to  define  the 
kinetostatic  conditioning  index  of  the  manipulator.  For  the  sake  of  brevity,  we  devote 
the  discussion  below  to  only  two  indices,  namely,  manipulability  and  conditioning. 
Prior  to  discussing  these  indices,  we  recall  a  few  facts  from  linear  algebra. 

Although  the  concepts  discussed  here  are  equally  applicable  to  square  and 
rectangular  matrices,  we  shall  focus  on  the  former.  First,  we  give  a  geometric 
interpretation  of  the  mapping  induced  by  a  x  ^  matrix  A.  Here,  we  do  not  assume 
any  particular  structure  of  A,  which  can  thus  be  totally  arbitrary.  However,  by 
invoking  the  polar-decomposition  theorem  (Strang  1988),  we  can  factor  A  as 

A  =  RU  =  VR  (5.73) 

where  R  is  orthogonal,  although  not  necessarily  proper,  while  U  and  V  are  both  at 
least  positive-semidefinite.  Moreover,  if  A  is  nonsingular,  then  U  and  V  are  both 
positive-definite,  and  R  is  unique.  Apparently, 

A^A  =  or  AA^  = 


(5.74) 
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and  hence,  U  (V)  can  be  readily  determined  as  the  positive-semidefinite  or 
correspondingly,  positive-definite  square  root  of  the  product  A^A  (AA^),  which 
is  necessarily  positive-semidefinite  at  least;  it  is,  in  fact,  positive-definite  if  A  is 
nonsingular.  We  recall  here  that  the  square  root  of  arbitrary  matrices  was  briefly 
discussed  in  Sect.  2.3.6.  The  square  root  of  a  positive-semidefinite  matrix  can 
be  most  easily  understood  if  that  matrix  is  assumed  to  be  in  diagonal  form, 
which  is  possible  because  such  a  matrix  is  necessarily  symmetric,  and  every 
symmetric  matrix  is  diagonalizable.  The  matrix  at  hand  being  positive-semidefinite, 
its  eigenvalues  are  nonnegative,  and  hence,  their  square  roots  are  all  real.  The 
positive-semidefinite  square  root  of  interest  is,  then,  readily  obtained  as  the  diagonal 
matrix  whose  nontrivial  entries  are  the  nonnegative  square  roots  of  the  above- 
mentioned  eigenvalues.  With  U  or  V  determined,  R  can  be  found  uniquely  only  if  A 
is  nonsingular,  in  which  case  U  and  V  are  positive-definite.  If  this  is  the  case,  then 
we  have 


R  =  AU“‘  =  V“‘A  (5.75a) 

It  is  a  simple  matter  to  show  that  U  and  V  are  related  by  a  similarity  transformation, 
namely. 


V  =  RUR^  (5.75b) 

Now,  as  a  consequence  of  the  above  relation  between  U  and  V,  both  matrices  share 
the  same  set  of  nonnegative  eigenvalues  {ct/  ,  which  are  termed  the  singular  values 
of  the  given  matrix  A.  Furthermore,  if  the  eigenvectors  of  U  are  denoted  by  {u/ 
and  those  of  V  by  {v/ ,  then  the  two  sets  are  related  by  a  similarity  transformation 
as  well: 


V/  =  Ru; ,  i  =  .  ,n  (5.76) 

Now,  let  vector  x  be  mapped  by  A  into  z,  i.e., 

z  =  Ax  =  RUx  (5.77a) 

Moreover,  let 

y  =  Ux  (5.77b) 

and  hence,  we  have  a  concatenation  of  mappings:  U  maps  x  into  y,  while  R  maps 
y  into  z.  Thus,  by  virtue  of  the  nature  of  matrices  R  and  U,  the  latter  maps  the  unit 
-dimensional  ball  into  a  /i-axis  ellipsoid  whose  semiaxis  lengths  bear  the  ratios 
of  the  eigenvalues  of  U.  Moreover,  R  maps  this  ellipsoid  into  another  one  with 
identical  semiaxes,  except  that  it  is  rotated  about  its  center  or  reflected,  depending 
upon  whether  R  is  proper  or  improper  orthogonal.  The  eigenvalues  of  U  or,  for  that 
matter,  those  of  V,  are  thus  nothing  but  the  singular  values  of  A.  Yoshikawa  (1985) 
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Fig.  5.9  Geometric  representation  of  mapping  induced  by  matrix  A 


explained  the  foregoing  relations  resorting  to  the  singular-value  decomposition 
theorem.  We  prefer  to  invoke  the  polar-decomposition  theorem  instead,  because  of 
the  geometric  nature  of  the  latter,  as  opposed  to  the  former,  which  is  of  an  algebraic 
nature — it  is  based  on  a  diagonalization  of  either  U  or  V,  which  is  really  not  needed. 

We  illustrate  the  two  mappings  U  and  R  in  Fig.  5.9,  where  we  orient  the  X, 
F,  and  Z  axes  along  the  three  eigenvectors  of  U.  Therefore,  the  semiaxes  of  the 
ellipsoid  are  oriented  as  the  eigenvectors  of  U  as  well.  If  A  is  singular,  then  the 
ellipsoid  degenerates  into  one  with  at  least  one  vanishing  semiaxis.  On  the  other 
hand,  if  matrix  A  is  isotropic,  i.e.,  if  all  its  singular  values  are  identical,  then  it  maps 
the  unit  ball  into  another  ball,  either  enlarged  or  shrunken. 

For  our  purposes,  we  can  regard  the  Jacobian  of  a  serial  manipulator  as  mapping 
the  unit  ball  in  the  space  of  joint  rates  into  a  rotated  or  reflected  ellipsoid  in  the  space 
of  Cartesian  velocities,  or  twists.  Now,  let  us  assume  that  the  polar  decomposition  of 
J  is  given  by  R  and  U,  the  manipulability  /x  of  the  robot  under  study  thus  becoming 

/X  =  I  det(J)  I  =  I  det(R)  1 1  det(U)  |  (5.78a) 

Since  R  is  orthogonal,  the  absolute  value  of  its  determinant  is  unity.  Additionally, 
the  determinant  of  U  is  nonnegative,  and  hence, 

/X  =  det(U)  (5.78b) 

which  shows  that  the  manipulability  is  the  product  of  the  eigenvalues  of  U  or 
equivalently,  of  the  singular  values  of  J.  Now,  the  product  of  those  singular  values, 
in  the  geometric  interpretation  of  the  mapping  induced  by  J,  is  proportional  to  the 
volume  of  the  ellipsoid  at  hand,  and  hence,  /x  can  be  interpreted  as  a  measure  of  the 
volume  of  that  ellipsoid.  It  is  apparent  that  the  manipulability  defined  in  Eq.  (5.78b) 
is  posture-dependent.  For  example,  if  J  is  singular,  at  least  one  of  the  semiaxes  of  the 
ellipsoid  vanishes,  and  so  does  its  volume.  Manipulators  at  singular  configurations 
thus  have  a  manipulability  of  zero. 
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Now,  if  we  want  to  use  the  concept  of  manipulability  to  define  a  posture- 
independent  kinetostatic  index,  we  have  to  define  this  index  in  a  global  sense.  This 
can  be  done  in  the  same  way  as  the  magnitude  of  a  vector  is  defined,  namely,  as 
the  sum  of  the  squares  of  its  components.  In  this  way,  the  global  manipulability 
can  be  defined  as  the  integral  of  a  certain  power  of  the  manipulability  over  the 
whole  workspace  of  the  manipulator,  which  would  amount  to  defining  the  index 
as  a  norm  of  the  manipulability  in  a  space  of  functions.^  For  example,  we  can  use 
the  maximum  manipulability  attained  over  the  whole  workspace,  thereby  ending  up 
with  what  would  be  like  a  Chebyshev  norrn^ ;  alternatively,  we  can  use  the  root-mean 
square  (rms)  value  of  the  manipulability,  thereby  ending  up  with  a  measure  similar 
to  the  Euclidean  norm. 

The  condition  number  of  a  square  matrix  is  a  measure  of  the  relative  roundoff- 
error  amplification  of  the  computed  results  upon  solving  a  linear  system  of  equations 
associated  with  that  matrix,  with  respect  to  the  relative  roundoff  error  of  the  data 
(Dahlquist  and  Bjorck  1974;  Golub  and  Van  Loan  1989).  Based  on  the  condition 
number  of  the  Jacobian,  a  posture-independent  kinetostatic  conditioning  index  of 
robotic  manipulators  can  be  defined  as  a  global  measure  of  the  condition  number. 

The  definition  of  the  condition  number  (Golub  and  Van  Loan  1989)  requires  that 
all  the  entries  of  the  matrix  at  hand  bear  the  same  physical  units,  which  we  assume 
first,  in  order  to  introduce  the  concept.  The  more  frequent  case  of  disparate  units 
will  be  treated  in  the  sequel.  The  condition  number  of  a  dimensionally  homogeneous 
Jacobian  J  is  defined  as 


^(J)  =  lUIIIU 


-1 


(5.79) 


where  ||  •  ||  stands  for  a  matrix  norm  (Golub  and  Van  Loan  1989).  While  any  norm 
can  be  used  in  the  above  definition,  the  one  that  is  most  convenient  for  our  purposes 
is  the  Frobenius  norm  ||  •  ll/?,  defined  as^ 


lUII^  =  J-tr(JF)  =  J-tr(V2)  (5.80a) 

\  n  y  n 

where  we  have  assumed  that  J  is  of  /2  xn.  Moreover,  from  the  polar-decomposition 
theorem  and  Theorem  2.6.4,  one  can  readily  verify  that 


^Lack  of  familiarity  with  the  mathematics  of  functions  regarded  as  elements  of  vector  spaces,  what 
is  C3\\&d  functional  analysis,  should  not  discourage  the  reader  from  continuing,  for  the  balance  of 
the  book  does  not  depend  on  these  concepts. 

norm  is  a  generalization  of  the  absolute  value  of  real  numbers,  but  applicable  to  arrays.  In  the 
same  way  that  a  vector  norm  is  a  measure  of  the  “size”  of  the  vector  components,  a  matrix  norm 
is  a  measure  of  the  “size”  of  the  matrix  entries.  In  this  vein,  the  Chebyshev  norm  of  a  given  vector 
(matrix)  is  the  largest  absolute  value  of  its  components  (entries). 

^Actually,  the  definition  of  Eq.  (5.80a)  yields  what  is  known  as  the  weighted  Frobenius  norm, 
which  gives  a  unit  norm  for  the  n  xn  identity  matrix,  regardless  of  the  value  of  n. 
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lUII^  =  J-tr(J^J)  =  J-tr(U2)  (5.80b) 

y  n  y  n 

Now,  since  the  trace  of  a  matrix  is  nothing  but  the  sum  of  the  matrix  eigenvalues, 
it  is  apparent  that  the  Frobenius  norm  is  identical  with  the  rms  value  of  the  set  of 
singular  values  of  the  given  matrix.  Likewise, 


tr(J-iJ-^) 


tr[(J^J)-‘] 


tr[(JJO-‘] 


(5.81) 


and  hence,  computing  the  Frobenius  norm  of  requires  the  inversion  not  of 
J  itself,  but  rather  that  of  J^J,  or  of  JJ^  for  that  matter.  Furthermore,  while  J 
is  not  frame-invariant  under  a  change  of  Cartesian-coordinate  frame,  JJ^  is.  As 
a  consequence,  the  latter  lends  itself  better  to  a  symbolic  inversion  than  J  itself. 
Hence,  the  Frobenius  condition  number  /c/r  is  derived  as 


kf  =  -  v'tr(U2)tr(U-2)  =  -  ytr(V2)tr(V-2) 
n  n 

Furthermore,  if  the  matrix  2-norm  is  used  in  definition  (5.79),  then 


JII2  =  max{a,}  =  am, 

i 


max 

i 


1 


where 


CTm  =  min{a, } 

i 


(5.82) 


(5.83a) 


(5.83b) 


It  is  noteworthy  that  both  the  Frobenius  norm  and  the  2-norm  are  given  in  terms  of 
the  matrix  singular  values.  As  a  consequence,  these  two  norms  are  frame-invariant. 
The  2-norm  condition  number  /C2(J)  is  thus  given  by 

/f2(J)  =  —  (5.84) 

Now  we  can  state  a  fundamental  result: 

Theorem  5.8.1.  The  condition  number  based  on  either  the  2-norm  or  the  Frobenius 
norm  of  the  robot  Jacobian  is  invariant  to  changes  of  frame.  In  this  light,  the 
said  condition  numbers  are  immutable  under  a  change  of  b\,  which  amounts  to 
a  translation  of  frame  J^i,  or  of  Oi,  which  amounts  to  looking  at  the  robot  from 
a  frame  rotated  by  this  angle  about  Z\.  Moreover,  angle  not  depending  on  the 
robot  architecture,  but  on  the  location  of  the  task  frame,  neither  influences  the  same 
condition  numbers. 
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Note  that,  regardless  of  the  norm  adopted,  the  condition  number  can  attain  values 
from  unity  to  infinity.  Apparently,  the  condition  number  attains  its  minimum  value 
of  unity  for  matrices  with  identical  singular  values;  such  matrices  map  the  unit 
ball  into  another  ball,  although  of  a  different  size,  and  are,  thus,  called  isotropic. 
By  extension,  isotropic  manipulators  are  those  whose  Jacobian  matrix  can  attain 
isotropic  values.  On  the  other  side  of  the  spectrum,  singular  matrices  have  a  smallest 
singular  value  that  vanishes,  and  hence,  their  condition  number  is  infinite.  The 
condition  number  of  J  can  be  thought  of  as  indicating  the  distortion  of  the  unit  ball 
in  the  space  of  joint- variables.  The  larger  this  distortion,  the  greater  the  condition 
number,  the  worst-conditioned  Jacobians  being  those  that  are  singular.  For  these, 
one  of  the  semiaxes  of  the  ellipsoid  vanishes  and  the  ellipsoid  degenerates  into  what 
would  amount  to  an  elliptical  disk  in  the  three-dimensional  space. 

Now,  if  the  entries  of  J  have  different  units,  its  condition  number  is  undefined, 
for  we  would  face  a  problem  of  either  adding  or  ordering  from  largest  to  smallest 
singular  values  of  different  units.  Staffetti  et  al.  (2002)  called  kinetostatic  per¬ 
formance  indices  of  manipulators  with  such  a  Jacobian  matrix  “non-invariant”  to 
changes  of  norms.  The  same  authors  went  on  to  claim  that,  because  of  this  feature, 
such  performance  indices — Staffetti  et  al.  refer  to  these  indices  as  “manipulability 
indices” — are  not  natural.  We  will  leave  aside  the  discussion  of  whether  the  indices 
at  stake  are  invariant  or  not,  to  focus  instead  on  means  to  cope  with  the  problem 
at  hand.  We  resolve  the  inconsistency  of  physical  units  by  defining  a  characteristic 
length,  by  which  we  divide  the  Jacobian  entries  that  have  units  of  length,  thereby 
producing  a  new  Jacobian  that  is  dimensionally  homogeneous.  We  shall  therefore 
divide  our  study  into  (a)  manipulators  for  only  positioning  tasks,  (b)  manipulators 
for  only  orientation  tasks,  and  (c)  manipulators  for  both  positioning  and  orientation 
tasks.  The  characteristic  length  will  be  introduced  when  studying  the  third  category. 

In  the  sequel,  we  will  need  an  important  property  of  isotropic  matrices  that  is 
recalled  below.  First  note  that  if  A  is  isotropic,  all  its  singular  values  are  identical, 
say  equal  to  cr,  and  hence,  matrices  U  and  V  are  proportional  to  the  n  y^n  identity 
matrix,  i.e.. 


(5.85) 


In  this  case,  then. 


A  =  (jR  (5.86a) 

which  means  that  isotropic  square  matrices  are  proportional  to  orthogonal  matrices. 
As  a  consequence,  then. 


A^A  =  AA^  =  (jH  (5.86b) 

Given  an  arbitrary  manipulator  of  the  serial  type  with  a  Jacobian  matrix  whose 
entries  all  have  the  same  units,  we  can  calculate  its  condition  number  and  use 
a  global  measure  of  this  to  define  a  posture-independent  kinetostatic  index.  Let 
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Km  be  the  minimum  value  attained  by  the  condition  number  of  the  dimensionally 
homogeneous  Jacobian  over  the  whole  workspace,  regardless  of  the  norm  adopted. 
Note  that  I/k^  can  be  regarded  as  a  Cheby  shev  norm^  of  the  reciprocal  of  the  condi¬ 
tion  number,  because  now  I/k^  represents  the  maximum  value  of  this  reciprocal  in 
the  whole  workspace.  We  then  introduce  a  posture-independent  performance  index, 
the  kinetostatic  conditioning  index,  or  KCI  for  brevity,  defined  as 

1 

KCI  =  —  X  100%  (5.87) 

Notice  that  since  the  condition  number  is  bounded  from  below,  the  KCI  is 
bounded  from  above  by  a  value  of  100%.  Manipulators  with  a  KCI  of  100%  are 
those  identified  above  as  isotropic. 


5.8.1  Positioning  Manipulators 

Here,  again,  we  shall  distinguish  between  planar  and  spatial  manipulators.  These 
are  studied  separately. 


Planar  Manipulators 

If  the  manipulator  of  Fig.  5.6  is  limited  to  positioning  tasks,  we  can  dispense  with  its 
third  axis,  the  manipulator  thus  reducing  to  the  one  shown  in  Fig.  5.7;  its  Jacobian 
reduces  correspondingly  to 


J  =  [Esi  ES2] 

with  Si  denoting  the  two-dimensional  versions  of  vectors  r^  of  the  Denavit- 
Hartenberg  notation,  as  introduced  in  Fig.  5.1.  Now,  if  we  want  to  design  this 
manipulator  for  maximum  manipulability,  we  resort  to  Eq.  (5.78a),  whence  /x  = 
|det(J)|.  First,  notice  that 

det(J)  =  det(E  [si  S2])  =  det(E)det([si  S2]) 

and  since  matrix  E  is  orthogonal,  its  determinant  equals  unity.  Thus,  the  determinant 
of  interest  is  now  calculated  using  Fact  5.7.3  of  Sect.  5.7,  namely, 

det(J)  = -s[Es2  (5.88) 


^In  a  space  of  functions. 
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Fig.  5.10  A  planar,  two-axis 
positioning  manipulator,  with 

6>i  =  0 


Therefore, 


/X  =  sfEs 


S1IIIIS2III  sin(si,  82)1 


where  (si,  S2)  stands  for  the  angle  between  the  two  vectors  inside  the  parentheses. 
Now  let  us  denote  the  manipulator  reach  with  R,  i.e.,  R  =  ai  a2,  and  let  ak  = 
Rpk,  where  fork  =  1, 2,  is  a  dimensionless  number.  As  illustrated  in  Fig.  5.10, 
IIS2II  I  sin(si,S2)|  =  h,  the  height  of  triangle  O1O2P  of  base  OiP,  and  hence,  /x 
turns  out  to  be  twice  the  area  of  the  same  triangle,  with  the  notation  adopted  at  the 
outset. 

Moreover,  in  terms  of  the  base  O1O2  =  ai  and  the  height  <22!  sin  ^2!,  the  area  of 
the  triangle  becomes  aia2  \  sin  02|/2,  and  hence. 


/X  =  <21(22 1  sin  02 1  =  P  Pi  Pi]  sin  O2 


(5.89a) 


with  Pi  and  p2  subject  to 


Pi  +  P2  —  1 


(5.89b) 


The  design  task  at  hand,  i.e.,  finding  <21  and  <22,  can  then  be  formulated  as  an 
optimization  problem  aimed  at  maximizing  /x  as  given  in  Eq.  (5.89a)  over  pi  and  p2, 
subject  to  the  constraint  (5.89b).  This  optimization  problem  can  be  readily  solved 
using,  for  example,  Lagrange  multipliers,  thereby  obtaining 


the  absolute  value  of  sin  02  attaining  its  maximum  value  when  02  =  ±90°.  The 
maximum  manipulability  thus  becomes 


/^max  — 


(5.90) 


Incidentally,  the  equal-length  condition  maximizes  the  workspace  volume  as  well. 
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Fig.  5.11  A  two-axis  isotropic  manipulator 


On  the  other  hand,  if  we  want  to  minimize  the  condition  number  of  J,  we 
should  aim  at  rendering  J  isotropic,  which  means  that  the  product  J  should  be 
proportional  to  the  identity  matrix,  and  so. 


V  T  r  1 

S;  Si  Sj  S2 

■a2 

o' 

T  T 

[s;  S2  S2  S2J 

_o 

where  a  is  the  repeated  singular  value  of  J.  Hence,  for  J  to  be  isotropic,  all  we  need 
is  that  the  two  vectors  Si  and  S2  have  the  same  norm  and  that  they  lie  at  right  angles. 
The  solution  is  a  manipulator  with  link  lengths  observing  a  ratio  of  \/2/2,  i.e.,  with 
aijai  =  \/2/2,  and  the  two  link  axes  at  an  angle  of  135°,  as  depicted  in  Fig.  5.11. 
Manipulators  of  the  above  type,  used  as  mechanical  fingers,  were  investigated  by 
Salisbury  and  Craig  (1982),  who  found  that  these  manipulators  can  be  rendered 
isotropic  if  given  the  foregoing  dimensions  and  configured  as  shown  in  Fig.  5.11. 


Spatial  Manipulators 

Now  we  have  a  manipulator  like  that  depicted  in  Fig.  4.1 1,  its  Jacobian  matrix  taking 
on  the  form 


J  =  [ei  X  ri  €2  X  r2  63  X  63]  (5.91) 

The  condition  for  isotropy  of  this  kind  of  manipulator  takes  on  the  form  of 
Eq.  (5.86b),  which  thus  leads  to 

lleixnp  (ei  X  ri)^(e2  X  r2)  (ei  X  ri)^(e3  X  rs) 

(ei  X  ri)^(e2  X  r2)  ||e2  x  r2|p  (62  x  r2)^(e3  x  rs) 

_(ei  X  ri)^(e3  x  63)  (62  x  r2)^(e3  x  63)  ||e3  x  r3f 


=  a^l  (5.92) 
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isotropy  circle 


Fig.  5.12  An  isotropic  manipulator  for  three-dimensional  positioning  tasks 


Hence,  the  manipulator  under  study  can  be  postured  so  as  to  attain  isotropy  if  its 
dimensions  are  chosen  so  that  its  three  columns  have  the  same  Euclidean  norm 
and  are  mutually  orthogonal.  These  conditions  can  be  attained  by  various  designs, 
one  example  being  the  manipulator  of  Fig.  4.17.  Another  isotropic  manipulator  for 
three-dimensional  positioning  tasks  is  displayed  in  Fig.  5.12. 

Note  that  the  manipulator  of  Fig.  5.12  has  an  orthogonal  architecture,  the  ratio 
of  its  last  link  length  to  the  length  of  the  intermediate  link  being,  as  in  the  two- 
dimensional  case,  \/2/2.  Since  the  first  axis  does  not  affect  singularities,  neither 
does  it  affect  isotropy,  and  hence,  not  only  does  one  location  of  the  operation  point 
exist  that  renders  the  manipulator  isotropic,  but  a  whole  locus,  namely,  the  circle 
known  as  the  isotropy  circle,  indicated  in  the  same  figure.  By  the  same  token,  the 
manipulator  of  Fig.  5.11  has  an  isotropy  circle  centered  at  the  center  of  the  first  joint, 
with  a  radius  of  (V2/2)ai. 


5.8.2  Orienting  Manipulators 

We  now  have  a  three-revolute  manipulator  like  that  depicted  in  Fig.  4.19,  its 
Jacobian  taking  on  the  simple  form 


J  =  [ei  €2  63] 


(5.93) 
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and  hence,  the  first  isotropy  condition  of  eq.  (5.86b)  leads  to 


J 


T 


T  T  T 

ef  ei  ef  02  e;  03 

T  T  T 

02  02  ^2  ^2  ^2  03 
T  T  T 
03  01  03  02  03  03 


(5.94) 


What  the  foregoing  condition  states  is  that  a  spherical  wrist  for  orienting  tasks 
is  isotropic  if  its  three  unit  vectors  {0^)1  are  so  laid  out  that  they  are  mutually 
orthogonal,  which  thus  yields  J  =  1,  the  3x3  identity  matrix.  Since  the  three 
singular  values  of  1  are  all  equal  to  unity,  i.e.,  a  =  I,  J^J  =  JJ^  =  1  as  well.  This 
is  the  case  in  orthogonal  wrists  when  the  two  planes  defined  by  the  corresponding 
pairs  of  neighboring  axes  are  at  right  angles.  In  summary,  then,  orthogonal  wrists, 
which  are  rather  frequent  among  industrial  manipulators,  are  isotropic.  Here  we 
have  an  example  of  engineering  insight  leading  to  an  optimum  design,  for  such 
wrists  existed  long  before  isotropy  was  introduced  as  a  manipulator  design  criterion. 
Moreover,  notice  that  from  the  results  of  Sect.  4.4.2,  spherical  manipulators  with 
an  orthogonal  architecture  have  a  maximum  workspace  volume.  That  is,  isotropic 
manipulators  of  the  spherical  type  have  two  optimality  properties:  they  have  both  a 
maximum  workspace  volume  and  a  maximum  KCI.  Apparently,  the  manipulability 
of  orthogonal  spherical  wrists  is  also  optimal,  as  the  reader  is  invited  to  verify,  when 
the  wrist  is  postured  so  that  its  three  axes  are  mutually  orthogonal.  In  this  posture, 
the  manipulability  of  the  wrist  is  unity. 


5.8.3  Positioning  and  Orienting  Manipulators 

We  saw  already  in  Sect.  5.8.1  that  the  optimization  of  the  two  indices  studied  here — 
the  Jacobian  condition  number  and  manipulability — leads  to  different  manipulators. 
In  fact,  the  two  indices  entail  even  deeper  differences,  as  we  shall  see  presently. 
First  and  foremost,  as  we  shall  prove  for  both  planar  and  spatial  manipulators, 
the  manipulability  /x  is  independent  of  the  operation  point  P  of  the  end-effector, 
while  the  condition  number  is  not.  One  more  fundamental  difference  is  that  while 
calculating  the  manipulability  of  manipulators  intended  for  both  positioning  and 
orienting  tasks  poses  no  problem,  the  condition  number  cannot  be  calculated,  at  least 
directly,  for  this  kind  of  manipulator.  Indeed,  in  order  to  determine  the  condition 
number  of  the  Jacobian  matrix,  we  must  either  add  or  order  from  largest  to  smallest 
its  singular  values.  However,  in  the  presence  of  positioning  and  orienting  tasks,  three 
of  these  singular  values,  those  associated  with  orientation,  are  dimensionless,  while 
those  associated  with  positioning  have  units  of  length,  thereby  making  impossible 
such  an  ordering.  We  resolve  this  dimensional  inhomogeneity  by  introducing  a 
normalizing  characteristic  length.  Upon  dividing  the  three  positioning  rows,  i.e.,  the 
bottom  rows,  of  the  Jacobian  by  this  length,  a  nondimensional  Jacobian  is  obtained 
whose  singular  values  are  nondimensional  as  well.  The  characteristic  length  is 
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then  defined  as  the  normalizing  length  that  renders  the  condition  number  of  the 
Jacobian  matrix  a  minimum.  While  this  definition  does  not  bear  a  direct  geometric 
interpretation,  in  general,  we  shall  see  that  such  an  interpretation  is  possible  for 
isotropic  manipulators.  Below  we  shall  determine  the  characteristic  length  for 
isotropic  manipulators;  determining  the  same  for  nonisotropic  manipulators  requires 
solving  a  minimization  problem  that  calls  for  numerical  techniques,  as  illustrated 
with  examples. 


Planar  Manipulators 


In  the  sequel,  we  will  need  the  planar  counterpart  of  the  twist-transfer  formula 
of  Sect.  3.4.2.  First,  we  denote  by  the  three-dimensional  twist  of  a  rigid  body 
undergoing  planar  motion — introduced  in  Eq.  (5.60) — when  defined  at  a  point  A; 
when  defined  at  point  B,  the  corresponding  twist  is  denoted  by  t^,  i.e.. 


(5.95) 


The  relation  between  the  two  twists,  or  the  planar  twist-transfer  formula,  is  given 
by  a  linear  transformation  U  as 


is  =  Ut^ 

where  U  is  now  defined  as  a  3  x  3  matrix,  namely, 

u = r  ^ 

E(b  -  a)  I2 


(5.96a) 


(5.96b) 


with  a  and  b  representing  the  position  vectors  of  points  A  and  B ,  and  I2  standing 
for  the  2x2  identity  matrix.  Moreover,  U  is,  not  surprisingly,  a  member  of  the  3x3 
unimodular  group,  i.e.. 


det(U)  =  1 

Because  of  the  planar  twist-transfer  formula,  the  Jacobian  defined  at  an  operation 
point  B  is  related  to  that  defined  at  an  operation  point  A  of  the  same  end-effector 
by  the  same  linear  transformation  U,  i.e.,  if  we  denote  the  two  Jacobians  by  and 
J  B ,  then 


iB  =  UJ^  (5.97) 

and  if  we  denote  by  pi  a  and  pis  the  manipulability  calculated  at  points  A  and  B, 
respectively,  then 


IX  B  =  |det(JB)|  =  |det(U)||det(J.4)|  =  |det(J.4)|  =  IJ^a 


(5.98) 
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thereby  proving  that  the  manipulability  is  insensitive  to  a  change  of  operation  point, 
or  to  a  change  of  end-effector,  for  that  matter.  Note  that  a  similar  analysis  for  the 
condition  number  cannot  be  completed  at  this  stage  because,  as  pointed  out  earlier, 
the  condition  number  of  these  Jacobian  matrices  cannot  even  be  calculated  directly. 

In  order  to  resolve  the  foregoing  dimensional  inhomogeneity,  we  introduce 
the  characteristic  length  L,  which  will  be  defined  as  that  rendering  the  Jacobian 
dimensionally  homogeneous  and  optimally  conditioned,  i.e.,  with  a  minimum 
condition  number.  We  thus  introduce  the  normalized  Jacobian  matrix  as 


1 

iEsi 


1  1 

fEs3 


(5.99) 


Now,  if  we  want  to  size  the  manipulator  at  hand  by  properly  choosing  its  geometric 
parameters  so  as  to  render  it  isotropic,  we  must  observe  the  isotropy  condition,  e.g., 
the  second  of  Eq.  (5.86b),  which  readily  leads  to 


and  hence. 


( V?  si)  E^  1 

Vo  o' 

v  — ^  ^  / 

— 

0  0 

_0  0  cr^_ 

(5.100) 


(5.101a) 

(5.101b) 

(5.101c) 


What  Eq.  (5.101a)  states  is  simply  that  the  triple  singular  value  of  the  isotropic  J 
is  \/3;  Eq.  (5.101b)  states,  in  turn,  that  the  operation  point  is  the  centroid  of  the 
centers  of  all  manipulator  joints  if  its  Jacobian  matrix  is  isotropic.  Now,  in  order  to 
gain  insight  into  Eq.  (5.101c),  we  note  that  since  E  is  orthogonal  and  =  3,  this 
equation  can  be  rewritten  in  a  simpler  form,  namely. 


^  =  (3)12  (5.102) 

Eurther,  if  we  recall  the  definition  of  the  moment  of  inertia  of  a  rigid  body,  we  can 
immediately  realize  that  the  moment  of  inertia  Ip  of  a.  set  of  particles  of  unit  mass 
located  at  the  centers  of  the  manipulator  joints,  with  respect  to  the  operation  point 
P ,  is  given  by 


I 


p 


(11*^ 


1 


(5.103) 
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from  which  it  is  apparent  that  the  moment  of  inertia  of  the  set  comprises  two  parts, 
the  first  being  isotropic — it  is  a  multiple  of  the  2x2  identity  matrix — the  second 
not  necessarily  so.  However,  the  second  part  has  the  form  of  the  left-hand  side  of 
Eq.  (5.102).  Hence,  Eq.  (5.102)  states  that  if  the  manipulator  under  study  is  isotropic, 
then  its  joint  centers  are  located,  at  the  isotropic  configuration,  at  the  comers  of  a 
triangle  that  has  circular  inertial  symmetry.  What  we  mean  by  this  is  that  the  2x2 
moment  of  inertia  of  the  set  of  particles,  with  entries  /^x,  Ixy^  and  lyy,  is  similar 
to  that  of  a  circle,  i.e.,  with  Ixx  =  lyy  and  Ixy  =  0.  An  obvious  candidate  is 
an  equilateral  triangle,  the  operation  point  thus  coinciding  with  the  center  of  the 
triangle.  Since  the  comers  of  an  equilateral  triangle  are  at  equal  distances  d  from 
the  center,  and  these  distances  are  nothing  but  ||s^  ||,  the  condition  below  is  readily 
derived  for  isotropy: 


\\skt  =  d\  A:  =1,2,3  (5.104) 

In  order  to  compute  the  characteristic  length  of  the  manipulator  under  study,  let  us 
take  the  trace  of  both  sides  of  Eq.  (5.102),  thereby  obtaining 

ii;iKip=6 

1 

and  hence,  upon  substituting  Eq.  (5.104)  into  the  foregoing  relation,  an  expression 
for  the  characteristic  length,  as  pertaining  to  planar  isotropic  manipulators,  is 
readily  derived: 


(5.105) 


It  is  now  a  simple  matter  to  show  that  the  three  link  lengths  of  this  isotropic 
manipulator  are  =  a2  =  \fid  and  a^  =  d.  Such  a  manipulator  is  sketched 
at  an  isotropic  posture  in  Eig.  5.13. 

We  now  can  give  a  geometric  interpretation  of  the  characteristic  length  for  the 
case  at  hand:  To  this  end,  we  look  at  the  manipulator  of  Eig.  5.13  from  an  arbitrary 
viewpoint  outside  of  the  manipulator  plane,  as  depicted  in  Eig.  5.4.  Let  this  plane  be 
X-Y ,  with  origin  at  Oi ,  and  X -axis  directed  towards  O2.  Next,  we  look  at  a  point  O 
on  the  normal  to  the  X-Y  plane  passing  through  the  operation  point  P ,  a  distance 
h  from  P  (Eig.  5.14). 

Eurther,  we  define  vectors  {r/  as 


r,  =  ^Oi,  i  =  1,2,3 


Upon  imposing  the  condition  that  the  set  {r^  be  orthogonal,  we  find  h  as 


=  L 


2 


(5.106) 
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Fig.  5.13  The  planar  3-R 
isotropic  manipulator 


Fig.  5.14  A  geometric  interpretation  of  the  characteristic  length  of  the  planar  3R  isotropic 
manipulator 


Therefore,  the  characteristic  length  L  renders  matrix  LJ  isotropic.  In  fact,  this 
matrix  becomes 


L  L  L 
ESi  ES2  ES3 


(5.107) 


for  {s/}^  valued  at  the  isotropic  posture  of  Fig.  5.13.  Notice  that  the  difference 
between  the  Jacobian  matrix  defined  in  Eq.  (5.60)  and  matrix  LJ  lies  only  in  their 
first  row.  Obviously,  the  former  is  not  dimensionally  homogeneous;  the  latter  is. 
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Spatial  Manipulators 

The  entries  of  the  Jacobian  of  a  six-axis  manipulator  intended  for  both  positioning 
and  orienting  tasks  are  dimensionally  inhomogeneous  as  well.  Indeed,  as  discussed 
in  Sect.  5.2,  the  /th  column  of  J  is  composed  of  the  Plucker  coordinates  of  the  /th 
axis  of  the  manipulator,  namely. 


Cl  02  03  ^4  05  06 

01  X  ri  02  X  r2  03  X  r3  04  X  r4  05  X  r5  06  X  r6 


(5.108) 


Now  it  is  apparent  that  the  first  three  rows  of  J  are  dimensionless,  whereas 
the  remaining  three,  corresponding  to  the  moments  of  the  axes  with  respect  to 
the  operation  point  of  the  end-effector,  have  units  of  length.  This  dimensional 
inhomogeneity  is  resolved  in  the  same  way  as  in  the  case  of  planar  manipulators 
for  both  positioning  and  orienting  tasks,  i.e.,  by  means  of  a  characteristic  length. 
This  length  is  defined,  again,  as  the  one  that  minimizes  the  condition  number  of  the 
dimensionless  Jacobian  thus  obtained.  We  then  define  the  corresponding  normalized 
Jacobian  as 


ei 

fei  X  n 


02  03  ^4  05  06 

f  62  X  r2  f  €3  X  r3  ^64  X  ^65  X  Ts  X  Te 


(5.109) 


and  hence,  the  second  isotropy  condition  of  Eq.  (5.86b)  leads  to 


6 


1 

(5.110a) 

6 

X  ri;)^  =  O 

1 

(5.110b) 

6 

'^(Ck  X  rk)(ek  X  ri;)^  =  aH 

(5.110c) 

1 


where  1  is  the  3x3  identity  matrix,  and  O  is  the  3x3  zero  matrix.  Now,  if  we  take 
the  trace  of  both  sides  of  Eq.  (5.1 10a),  we  obtain 

=  2  or  G  =  sPi 


Eurthermore,  we  take  the  trace  of  both  sides  of  Eq.  (5.110c),  which  yields 
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But  ||e^  X  Ip  is  nothing  but  the  square  of  the  distance  dk  of  the  /:th  revolute  axis 
to  the  operation  point,  the  foregoing  equation  thus  yielding 


i.e.,  the  characteristic  length  of  a  spatial  six-revolute  isotropic  manipulator  is  the 
root-mean  square  of  the  distances  of  the  revolute  axes  to  the  operation  point  when 
the  robot  finds  itself  at  the  posture  of  minimum  condition  number. 

Furthermore,  Eq.  (5.110a)  states  that  if  {e^  is  regarded  as  the  set  of  position 
vectors  of  points  {Pk}i  on  the  surface  of  the  unit  sphere,  then  the  moment-of-inertia 
matrix  of  the  set  of  equal  masses  located  at  these  points  has  spherical  symmetry. 
What  the  latter  means  is  that  any  direction  of  the  three-dimensional  space  is  a 
principal  axis  of  inertia  of  the  foregoing  set.  Likewise,  Eq.  (5.110c)  states  that  if 
{  X  is  regarded  as  the  set  of  position  vectors  of  points  {  }  in  the  three- 

dimensional  Euclidean  space,  then  the  moment-of-inertia  matrix  of  the  set  of  equal 
masses  located  at  these  points  has  spherical  symmetry  as  well. 

Now,  in  order  to  gain  insight  into  Eq.  (5.1 10b),  let  us  take  the  axial  vector  of  both 
sides  of  that  equation,  thus  obtaining 

6 

X  (Cfc  X  r,t)  =  0  (5.111) 

1 

with  0  denoting  the  three-dimensional  zero  vector.  Furthermore,  let  us  denote  by 
the  cross-product  matrix  of  ,  the  foregoing  equation  thus  taking  the  form 

EE2r,=0 

1 


However, 


=  -1  +  e,e[ 

for  every  k,  and  hence,  Eq.  (5.111)  leads  to 

6 

E(l-e<:e[)ri;  =  0 

1 

Moreover,  (1  —  e^e^)r^  is  nothing  but  the  normal  component  of  with  respect  to 
e^,  as  defined  in  Sect.  2.2.  Let  us  denote  this  component  by  r^,  thereby  obtaining 
an  alternative  expression  for  the  foregoing  equation,  namely, 

Ev"  =  o 

1 


(5.112) 
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Fig.  5.15  DIESTRO,  a  six-axis  isotropic  manipulator  in  two  postures:  (a)  with  the  arm  extended 
upwards;  (b)  with  the  arm  down,  showing  the  orthogonality  of  the  neighboring  axes 


Table  5.1  DH  parameters  of 
DIESTRO 


i 

Qi  (mm) 

bi  (mm) 

Oti 

1 

50 

50 

o 

o 

ei 

2 

50 

50 

1 

o 

o 

^2 

3 

50 

50 

o 

o 

^3 

4 

50 

50 

o 

O 

o^ 

1 

5 

50 

50 

o 

o 

Os 

6 

50 

50 

1 

o 

o 

Oe 

The  geometric  interpretation  of  the  above  equation  is  now  apparent:  Let  be  the 
foot  of  the  perpendicular  to  the  ki\\  revolute  axis  from  the  operation  point  P ;  then, 
Yk  is  the  vector  directed  from  O'j^io  P .  Therefore,  the  operation  point  of  an  isotropic 
manipulator,  configured  at  the  isotropic  posture,  is  the  centroid  of  the  set  {  of 
perpendicular  feet  from  the  operation  point. 

An  axis  layout  that  satisfies  the  foregoing  isotropy  conditions  is  obtained  when 
the  six  axes  are  laid  out  along  the  diagonals  of  the  faces  of  a  cube.  If  the  side  of 
the  cube  has  a  length  a,  then  all  distances  dk  from  the  cube  centroid  to  the  axes  are 
identical:  dk  =  a  j 2,  fox  k  =  1 , . . . ,  6. 

A  six-axis  manipulator  designed  with  an  isotropic  architecture,  DIESTRO,  is 
displayed  in  Fig.  5.15.  The  Denavit-Hartenberg  parameters  of  this  manipulator  are 
given  in  Table  5.1.  DIESTRO  is  characterized  by  identical  link  lengths  a  and  offsets 
identical  with  this  common  link  length,  besides  twist  angles  of  90°  between  all  pairs 
of  neighboring  axes.  Not  surprisingly,  the  characteristic  length  of  this  manipulator 


IS  a. 
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Fig.  5.16  Equilateral, 
three-revolute  planar  robot 


P 


5.8.4  Computation  of  the  Characteristic  Length:  Applications 
to  Performance  Evaluation 

We  elaborate  further  on  the  concept  of  characteristic  length.  In  order  to  provide  a 
better  grasp  of  the  concept,  we  focus  on  its  computation  as  pertaining  to  a  given 
robot,  that  need  not  be  isotropic.  To  do  this,  we  include  two  examples,  one  planar 
and  one  spatial,  industrial  robot.  Once  a  numerical  value  of  the  characteristic  length 
is  available,  we  can  compute  the  minimum  value  of  the  condition  number  of  the 
robot  Jacobian,  with  which  we  can  assess  the  robot  kinetostatic  performance  by 
means  of  the  KCI. 

Example  5.8.1  (A  Planar,  Equilateral,  Three -Revolute  Robot).  Compute  the  char¬ 
acteristic  length  of  the  robot  of  Fig.  5.16,  depicting  a  posture  in  which  0i  has  been 
set  equal  to  zero.  What  is  the  KCI  of  this  robot? 

Solution:  We  have  a\  =  a2  =  a^  =  a  for  the  robot  under  study.  In  order  to 
compute  its  length,  we  have  to  minimize  the  Jacobian  condition  number  by  a  proper 
choice  of  the  characteristic  length  L  and  the  joint  variables  O2  and  O3.  We  thus  start 
by  deriving  an  expression  for  the  Jacobian: 


ESi  ES2  ES3 


Now,  we  render  J  dimensionally  homogeneous  by  introducing  the  characteristic 
length  L,  thus  obtaining  the  normalized  Jacobian  J: 


J  = 


1 


1 


1 


(l/L)Esi  (1/L)Es2  (1/L)Es3 
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From  the  manipulator  geometry,  we  have, 


II 

—  (^1  +  ^12  +  ^123) 

,  Es2  =  a 

—(^12  +  ^123) 

,  Es3  =  a 

—^123 

Cl  +  C12  +  C123 

<^12  +  <^123 

<^123 

with 


Cl  =  cos  01,  C2  =  sin  02,  <^12  =  cos(0i  +  02),  Ci23  =  COS(0i  +  02  +  O3) 

Si  =  sin  01,  ^2  =  sin  02,  ^12  =  sin(0i  +  02),  ^123  =  sin(0i  +  02  +  03) 

Since  we  set  0i  =  0,  because  the  first  joint  does  not  affect  the  condition  number,  the 
normalized  Jacobian  becomes 


1  1  1 


-r(s2  +  ^23)  -r(s2  +  ^23)  -rs23  , 

_r(l  +  C2  +  C23)  r(c2  +  C23)  rc23  _ 


r  = 


a 

L 


the  inverse  of  J,  as  derived  with  computer  algebra,  being 


S3/S2  C2/(rs2)  1/r 

-fc  +  ^23)7^2  -(1  +  C2)/(rs2)  -1/r 
_  (^2+ ^23)7^2  l/(^^2)  0  _ 


The  square  of  the  Frobenius-norm  condition  number  of  J  is  now  computed  as 

=  fg 


with  /  and  g  defined  as  the  square  of  the  weighted  Frobenius  norms  of  J  and  J 
respectively,  i.e.. 


1 

3 


tr(J^J)  =  1  + 


2  4  2  \ 

2  +  + -c,  +  -cn\ 


r 


2 


and 


fe  +  ^23)^  +  fc  +  ^23)^  +  ^3  I  2  2  +  C2 
?>s\  3  r'^s^ 


f  =  Ar'^ 


which  can  be  rewritten  as 
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and 


1  D  +  E/r^ 

S  X  2 
3 

with  coefficients  A,  D  and  E  independent  of  r,  namely, 

2 

A  =  -(3  +  C2  +  2c3  +  C23) 

D  =  {s2  -\-  ^23)^  +  fc  +  ^23)^  +  ^3 

^  ^  2(2  +  C2) 


We  now  have  a  classical  minimization  problem: 


kI  =  fg  =  ^(l+Ar^) 


D  +  Ejr 


min 

r, 62,63 


where  the  characteristic  length  is  implicit  in  r.  While  the  foregoing  problem  is  well 
posed,  we  should  not  forget  that  kf  is  unbounded  from  above.  In  order  to  gain 
better  insight  into  the  problem  at  hand,  it  is  preferable  to  treat  the  problem  as  one 
of  maximization  of  l/zc/?,  or  of  its  square,  for  that  matter.  As  well,  we  can  dispense 
with  the  constant  factor  1  /  3  in  ,  which  thus  leads  to  the  maximization  problem 
below: 


max 

^,62,63 


with  P  and  Q  defined  as 

P  =  ADr*  +  {AE  +  D)r^  +  E,  Q= 


In  order  to  obtain  the  optimum  values  of  the  three  design  variables  r,  O2,  and 
O3,  we  need  to  set  up  the  normality  conditions  of  the  problem  at  hand.  These  are 
readily  obtained  upon  zeroing  the  gradient  of  k\  with  respect  to  the  vector  of  design 

variables,  defined  as  x  =  [  r  02  ^3  ]  •  The  said  conditions  are,  thus. 


dz 

9x 


9  (Q 

P 


9x 


The  three  components  of  the  above  gradient,  dzidr,  9z/902,  and  9z/903,  are  then 
derived  using  the  general  formula  for  the  derivative  of  a  rational  expression: 


9 

dxi 


1 

~P^ 


(QiP 


QPi) 
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where  Qi  and  Pi  stand  for  dQjdxt  and  dP /dxt,  with  Xf  taking  values  of  r,  O2, 
and  03,  for  /  =  1,2,3,  respectively.  We  thus  have,  using  a  similar  notation  for  the 
partial  derivatives  of  coefficients  A,  D  and  E: 

Pi  =  4ADP  +  2(AE  +  D)r,  Qi  =  Irsj 

P2  =  (A2D  +  AD2)r^  +  (A2E  +  AE2  +  D2)r^  +  E2,  Q2  —  2r^S2C2 

P3  =  (A2D  +  AD^y  +  {A2E  +  AE2  +  D^y  +  £3,  03  =  0 

Apparently,  =  0,  the  normality  conditions  thus  simplifying  to 

az  ^  _2^  +  £)  =  0 

dr  V  ^  I 

a  2  2 

{[2ADc2  -  (A2D  +  AD2)s2y  +  [2(AE  +  D)c2 

OU2  P 

-{A2E  +  AE2  +  £>2)^2]  +  2Ec2  -  £’2^2}  =  0 

a  4  2 

^  ^  +  (^3£  +  £>3)]  =  0 

thereby  obtaining  a  system  of  three  nonlinear  equations  in  three  unknowns,  namely, 
the  three  design  variables.  Apparently,  all  three  normality  conditions  are  satisfied 
for  either  r  =  0  or  ^2  =  0,  which  just  confirms  that  the  normality  conditions  are 
sufficient  for  a  point  in  the  design  space  to  be  stationary  \  such  a  point  can  be  a  local 
minimum,  a  local  maximum  or  a  saddle  point.  The  vanishing  of  the  product  rs2  thus 
yields  a  minimum  of  z,  which  indicates  Kp  00,  r  =  0  giving  an  architecture 
singularity,  S2  =  0  sl  posture  singularity.  We  are  not  interested,  for  our  purposes,  on 
such  a  minimum,  for  which  reason  we  assume  henceforth  that  rs2  0.  Under  this 
condition,  the  normality  conditions  thus  yield  the  reduced  system  of  equations 

01  =  —ADr^-\-E=^ 

(P2  =  [2ADc2-(A2D+AD2)s2y+[2iAE  +  D)c2-(A2E+AE2+D2)s2y 

-\-2Ec2  -  E2S2  =  0 

03  =  (^3^  +  AD2)r^  +  {^A^E  +  D3)  =  0 

The  problem  at  hand  is  thus  solved  by  finding  the  roots  of  the  foregoing  system. 
We  can  do  this  by  means  of  the  Newton-Raphson  method,  for  example,  which  (a) 
requires  the  3x3  Jacobian  matrix  of  the  three  foregoing  equations,  i.e.,  further 
differentiation,  and  (b)  yields  only  one  root,  out  of  many,  for  one  given  initial  guess, 
when  the  method  converges  at  all.  Moreover,  given  the  local  nature  of  the  method, 
Newton-Raphson  cannot  tell  whether  one  has  found  all  possible  roots  of  the  system 
of  equations. 
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An  alternative,  semigraphical  approach,  was  introduced  in  Example  4.4.3.  This 
approach  consists  in  reducing  the  problem  to  finding  the  roots  of  two  nonlinear 
equations  in  two  unknowns;  each  equation,  then,  defines  one  contour  in  the  plane 
of  the  two  unknowns,  the  intersection  points  of  the  two  contours  yielding  all 
possible  real  roots  of  the  system  at  hand.  In  order  to  apply  this  approach  to  the 
above  system,  we  have  to  eliminate  one  of  the  three  unknowns  from  the  system, 
the  obvious  candidate  being  r.  We  can  do  this  by  dialytic  elimination,  as  introduced 
in  Sect.  5.4.1.  Given  the  special  structure  of  the  three  given  equations,  it  is  simpler 
to  eliminate  r  following  a  straightforward  approach:  First,  we  solve  for  from  the 
first  equation  and  for  from  the  third,  which  yields: 

^  E  2  ^?,E  -\- 

^  “  Zd’  ^  “  ~  A3D  +  AD3 

upon  equating  the  above  expression  of  with  the  square  of  its  counterpart 
expression  for  r^,  we  obtain 

F(02,  O3)  =  (AE  -  D){ADl  -  AjDE)  =  0 

Further,  upon  substituting  the  same  expressions  for  and  into  equation  (p2  =  0, 
we  obtain 

G(^2,  O3)  =  {AE  -  D)[{AA3DE2  +  A2A3DE  -  AD2D3)  sin  62 
-2AD{A3E  -  D3)  cos  O2]  =  0 

thereby  obtaining  a  reduced  system  of  two  equations  in  two  unknowns  only,  O2  and 
O3.  The  foregoing  system  admits  further  simplifications.  Indeed,  the  common  factor 
AE  —  D  turns  out  to  be  positive-definite,  i.e.,  AE  —  D  >  Ofor  any  values  of  O2 
and  O3.  While  it  is  not  obvious  that  the  factor  in  question  is  positive-definite,  its 
sign-definiteness  was  verified  with  the  aid  of  computer  algebra.  To  visualize  this 
property,  we  include  a  three-dimensional  rendering  of  the  function  as  a  surface  in 
Fig.  5. 17a  and  a  side  view  of  the  same  in  Fig.  5. 17b.  Given  that  the  factor  in  question 
is  positive-definite,  we  can  safely  divide  both  sides  of  the  two  foregoing  equations 
by  this  factor,  which  thus  leads  to  two  nonlinear  equations  in  O2  and  O3  defining 
contours  Ci  and  C2  in  the  O2-O3  plane,  namely. 

Cl  :  ADI-A\DE  =  0 

C2  •  {AA3T)E2  ~\~  A2A3DE  —  ^47)2^3)  sin  O2  —  2AD{A3E  —  7)3)  cos  O2  —  0 

The  two  above  contours  are  plotted  in  the  O2-O3  plane  in  Fig.  5.18. 

Apparently,  to  any  optimum  posture  with  joint  center  O3  above  line  O1O2 
corresponds  a  symmetrically  located  posture  of  the  robot  with  O3  below  the  above 
line.  This  means  that  all  solutions  (O2,  O3)  expected  should  be  symmetric  about  the 
origin  of  the  O2-O3  plane,  which  they  are,  as  illustrated  in  Fig.  5.18a.  That  is,  if  a 
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Fig.  5.17  A  3D  rendering  of  the  function  ^  =  AE  —  D  \s.  62  and  Oy.  (a)  an  isometric  view;  and 
(b)  a  view  in  the  62-^  plane 


Fig.  5.18  Contours  C\  and  C2  in  the  02-^3  plane:  (a)  in  the  —Tt  <  O2  <  n,  —ic  <  0^  <  n  range; 
(b)  a  zoom-in  in  the  0<  62  <Jt,0<  63  <Jt  range 


pair  of  numerical  values  (O2,  O^)  verifies  the  normality  conditions,  then  so  does  the 
pair  (—O2,  —O3).  By  the  same  token,  if  we  set  =  tt  in  the  Jacobian  expression, 
a  similar  set  of  normality  conditions  should  be  obtained,  with  the  corresponding 
symmetry. 

In  light  of  the  symmetry  of  the  plots  of  Fig.  5.18a,  we  can  focus  on  the  first 
quadrant  of  the  O2-O3  plane,  and  plot  the  zoom-in  of  Fig.  5.18a,  showing  only  this 
quadrant.  Moreover,  it  is  apparent  that  contour  Ci  exhibits  two  double  points,  one 
at  (0.9,  2.9),  the  other  at  (2.1,  2.6).  Double  points  are  likely  to  produce  spurious 
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solutions'^;  hence,  we  discard  those  two  double  points,  thereby  leaving  only  five 
intersections  of  interest.  As  it  turns  out,  the  intersection  detected  by  inspection  at, 
roughly,  O2  =  1.69  rad,  O3  =  2.61  rad  produces  a  maximum  of  l/zc/?.  These  rough 
values  of  the  design  variables  were  then  refined  using  the  Newton-Raphson  method, 
with  the  foregoing  rough  values  as  initial  guess. The  Newton-Raphson  method, 
as  implemented  with  Matlab  code,  yielded  the  refined  solution  displayed  below  in 
eight  iterations: 


O2  =  1.68910726900188  rad  =  96.11^11163° , 

O3  =  2.61287852677543  rad  =  149.7069120°, 
r  =  2.040896177  ^  L  =  a/r  =  0.4899808287  m 

where  we  have  recalled  that  a  was  specified  as  1  m.  The  normalized  Jacobian  J  at 
the  optimum  posture  is,  moreover. 


1  1  1 
-0.1552  -0.1552  1.8715 
0.9858  -1.0552  -0.8143 


with/c/7  =  1.1832.  Hence, 


KCI  =  84.52% 


Note  that,  if  we  use  the  2-norm  to  define  the  condition  number,  i.e.,  if  we  minimize 


Kiir,  02, 63) 


then  we  cannot  find  expressions  for  the  gradient  oHIk^  with  respect  to  the  design 
variables  because  the  objective  function  now  is  not  analytic^^  in  the  design  variables. 
That  is,  unlike  the  minimization  of  Kp,  now  we  do  not  have  normality  conditions. 
Nevertheless,  K2  can  still  be  minimized  using  a  direct-search  method,  i.e.,  an 


^  A  spurious  solution  is  a  set  of  numerical  values  of  the  roots  of  a  system  of  equations  that,  although 
computed  from  a  sound  elimination  procedure,  does  not  verify  the  equations.  Example  9.7.3 
includes  a  case  of  a  double  point  in  a  contour  that  yields  spurious  solutions. 

^^It  is  well  known  (Dahlquist  and  Bjorck  1974)  that,  close  to  a  root,  the  Newton-Raphson  method 
converges  quadratically,  i.e.,  the  approximation  to  the  root  gains,  roughly,  two  digits  of  accuracy 
at  each  iteration.  Hence,  the  Newton-Raphson  procedure  will  likely  converge  to  the  root  closest  to 
the  given  estimate. 

A  real- valued  function  of  a  real  argument  is  said  to  be  analytic  at  one  value  of  its  argument  if  the 
function  admits  a  series  expansion  at  this  value.  To  be  true,  the  exception  to  the  rule  is  the  case  of 
2x2  matrices,  which  have  an  analytic  K2. 
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Fig.  5.19  Optimum 
configuration  for  a 
minimum  Kf 


optimization  method  not  relying  on  gradients,  but  only  on  objective-function  evalu¬ 
ation.  The  objective  function  is  that  whose  minimum,  or  maximum  for  that  matter, 
is  searched.  In  our  case,  the  objective  function  to  minimize  is  K2.  Direct-search 
minimization  methods  are  available  in  scientific  software.  Matlab,  for  example,  uses 
the  Nelder-Mead  simplex  method,  as  implemented  in  its  fminsearch  function. 
A  local  minimum  value  of  K2  was  found  by  fminsearch  with  the  initial  guess 

r  =  1.0,  62  =  6.0°,  6>3  =  18.0° 


after  148  iterations,  as 


K2  =  1.9070 


This  value  is  attained  at  the  values  of  the  design  variables  given  below: 

r  =  2.1650,  62  =  98.9785°,  0^  =  145.193° 


which  yield  L  =  0.4619  m  and  a  nondimensional  Jacobian 


1  1  1 
-0.1898  -0.1898  1.9488 
0.8839  -1.2813  -0.9433 


Shown  in  Fig.  5.19  is  the  given  manipulator  at  the  optimum  posture  under  the 
condition  number  calculated  using  the  Frobenius  norm,  the  posture  corresponding  to 
the  minimum  condition  number  based  on  the  2-norm  being  indistinguishable  from 
this  one. 

Example  5.8.2.  Find  the  KCI  and  the  characteristic  length  of  the  Fanuc  Robot  Arc 
Mate  S  series  manipulator  of  1990,  whose  DH  parameters  are  given  in  Table  5.2. 

Solution:  We  need  the  minimum  value  that  the  condition  number  kf  of  the 
normalized  robot  Jacobian  can  attain,  in  order  to  calculate  its  KCI  as  indicated 
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Table  5.2  DH  parameters  of 
the  Fanuc  Arc  Mate  S  series 
manipulator  of  1990 
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in  Eq.  (5.87).  Now,  the  Fanuc  Robot  Arc  Mate  S  series  of  1990  is  a  six-revolute 
manipulator  for  positioning  and  orienting  tasks.  Hence,  its  Jacobian  matrix  has  to 
be  first  recast  in  nondimensional  form,  as  in  Eq.  (5.109).  Next,  we  find  L  along  with 
the  joint  variables  that  determine  the  posture  of  minimum  condition  number  via 
an  optimization  procedure.  Prior  to  the  formulation  of  the  underlying  optimization 
problem,  however,  we  recall  Theorem  5.8.1,  under  which  the  first  joint,  accounting 
for  motions  of  the  manipulator  as  a  single  rigid  body,  does  not  affect  its  Jacobian 
condition  number.  By  the  same  token,  we  align  axes  and  Zq.  consequence, 
^6  does  not  affect  the  Jacobian  condition  number  either.  We  thus  define  the  design 
vector  X  of  the  optimization  problem  at  hand  as  a  five-dimensional  array,  namely, 

X  =  [^2  ^3  ^4  ^5 

and  set  up  the  optimization  problem  as 

min  Kf  (j) 

X 

Now,  given  the  architecture  of  the  robot  at  hand,  a  symbolic  expression  for  or 

its  dimensionless  counterpart  J  ,  not  to  speak  of  kf  itself,  is  elusive,  and  hence, 
an  approach  like  that  of  Example  5.8.1  appears  rather  unfeasible.  We  thus  resort  to 
a  direct-search — as  opposed  to  a  gradient-based — procedure  to  solve  the  foregoing 
optimization  problem.  There  are  various  methods  of  this  kind  at  our  disposal;  the 
one  we  chose  is,  again,  the  Nelder-Mead  simplex  method,  as  implemented  in  Matlab 
within  the  fminsearch  function,  which  was  provided  with  the  initial  guess 

Xinit  =  [26°  -56°  195°  107°  341.738]^ 

The  results  reported  by  Matlab  are  displayed  below: 

Xopt  =  [22.60°  -51.13°  -159.93°  88°  351.23]^ 
whose  last  entry,  the  characteristic  length  of  the  robot,  is  in  mm,  i.e.. 


L  =  351.23  mm 
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Furthermore,  the  minimum  condition  number  attained  at  the  foregoing  posture,  with 
the  characteristic  length  found  above,  is 


kf  =  1.2717 

Therefore,  the  KCI  of  the  Fanuc  Robot  Arc  Mate  S  series  manipulator  of  1990  is 

KCI  =  78.63% 

To  be  sure,  the  KCI  of  this  manipulator  can  still  be  improved  dramatically  by  noting 
that  the  condition  number  is  highly  dependent  on  the  location  of  the  operation  point 
of  the  end-effector.  The  robot  DH  parameters  given  in  Table  5.2  do  not  account  for 
the  geometry  of  the  EE. 


5.9  Exercises 

N.B.:  Exercises  5.14-5.18  pertain  to  Sect.  5.8.  They  are  thus  to  be  assigned  only 
if  this  section  was  covered  in  class. 

5.1  Shown  in  Fig.  5.20  is  a  computer-generated  model  of  DIESTRO,  the  robot 
displayed  in  Fig.  5.15,  with  a  slightly  modified  EE.  The  Denavit-Hartenberg 
parameters  of  this  robot  are  given  in  Table  5.3.  Find  the  Jacobian  matrix  of  the 
manipulator  at  the  above  configuration. 


Fig.  5.20  A  six-revolute 
manipulator 
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Table  5.3  DH  parameters  of 
the  modified  DIESTRO 


Fig.  5.21  A  planar  three-axis 
manipulator 


i 

ai  (mm) 

bi  (mm) 

Oli 

ft 

1 

50 

50 

0 

0 

o^ 

0 

0 

2 

50 

50 

1 

0 

0 

1 

0 

0 

3 

50 

50 

0 

0 

0 

0 

4 

50 

50 

1 

0 

0 

0 

0 

o^ 

1 

5 

50 

50 

0 

0 

0 

0 

6 

0 

50 

1 

0 

0 

0 

0 

o^ 

1 

P(x,y) 


5.2  The  robotic  manipulator  of  Fig.  4.23  is  instrumented  with  sensors  measuring 
the  torque  applied  by  the  motors  at  the  joints.  Two  readouts  are  taken  of  the 
sensors  for  the  robot  in  the  configuration  indicated  in  the  figure.  In  the  first 
readout,  the  gripper  is  empty;  in  the  second,  it  holds  a  tool.  If  the  first  readout 
is  subtracted  from  the  second,  the  vector  difference  At  is  obtained  as 

At  =  [O  2  1  0  1  O]^  Nm 

With  the  foregoing  information,  determine  the  weight  w  of  the  tool  and  the 
distance  d  of  its  mass  center  from  C ,  the  center  of  the  spherical  wrist. 

5.3  A  planar  three-axis  manipulator  is  shown  in  Fig.  5.21,  with  ai  =  a2  =  = 

1  m.  When  a  wrench  acts  onto  the  EE  of  this  manipulator,  the  joint  motors 
exert  torques  that  keep  the  manipulator  under  static  equilibrium.  Readouts  of 
these  joint  torques  are  recorded  when  the  manipulator  is  in  the  posture  0i  = 
O2  =  O3  =  45°,  namely, 

n  =  —V2  Nm,  T2  =  —V2  Nm,  =  I  —  Vl  Nm 


Calculate  the  above-mentioned  wrench. 
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5.4  For  the  two  postures  found  in  Exercise  4.8,  the  EE  is  to  move  with  an  angular 

velocity  o)  =  [coi,  0)2,  <^3]^  Show  that  if  ||a)||  remains  constant,  then  so 

•  • 

does  11^  II,  for  ^  defined  as  the  joint-rate  vector  of  the  wrist. 

5.5  Point  C  of  the  manipulator  of  Fig.  4.17  is  to  move  with  a  velocity  v  in  the 

posture  displayed  in  that  figure.  Show  that  as  long  as  ||v||  remains  constant, 

•  • 

so  does  11^  II,  for  0  defined  as  the  joint-rate  vector.  Moreover,  let  us  assume 

that  in  the  same  posture,  point  C  is  to  attain  a  given  acceleration  a.  In  general, 

•  •  •  • 

however,  ||^||,  where  0  denotes  the  corresponding  joint- acceleration  vector, 

does  not  necessarily  remain  constant  under  a  constant  ||a||.  Under  which 

•  • 

conditions  does  ||a||  remain  constant  for  a  constant  ||^  ||? 

5.6  A  load  f  is  applied  to  the  manipulator  of  Fig.  4.17  in  the  posture  displayed  in 
that  figure.  Torque  cells  at  the  joints  are  calibrated  to  supply  torque  readouts 
resulting  from  this  load  only,  and  not  from  the  dead  load — its  own  weight — of 
the  manipulator.  Show  that  under  a  constant-magnitude  load,  the  magnitude 
of  the  joint-torque  vector  remains  constant  as  well. 

5.7  Shown  in  Fig.  4.24  is  the  kinematic  chain  of  an  industrial  robot,  like  the  ABB- 
IRB  1000,  which  contains  five  revolutes  and  one  prismatic  pair. 

(a)  Determine  the  manipulator  Jacobian  in  the  Ai,  Fi,  Zi  coordinate  frame 
fixed  to  the  base. 

(b)  Determine  the  twist  of  the  end-effector,  defined  in  terms  of  the  velocity  of 
point  P,  for  unit  values  of  all  joint-rates,  and  the  posture  displayed  in  the 
same  figure. 

(c)  Determine  the  joint  accelerations  that  will  produce  a  vanishing  accelera¬ 
tion  of  the  point  of  intersection,  C ,  of  the  three  wrist  axes  and  a  vanishing 
angular  acceleration  of  the  gripper,  for  the  unit  joint  rates  given  above. 

5.8  The  robot  in  Fig.  4.24  is  now  used  for  a  debarring  task.  When  the  robot  is  in 
the  configuration  shown  in  that  figure,  a  static  force  f  and  no  moment  acts  on 
point  P  of  the  debarring  tool.  This  force  is  sensed  by  torque  sensors  placed  at 
the  joints  of  the  robot.  Assume  that  the  distance  between  the  operating  point 
P  and  the  wrist  center  is  500  mm  and  that  the  readings  of  the  arm  joints  are 
ri  =  0,  r2  =  100  N  m,  and  13  =  50  N  m. 

(a)  Find  the  force  f  acting  at  P . 

(b)  Find  the  readings  of  the  torque  sensors  placed  at  the  wrist  joints. 

5.9  A  decoupled  manipulator  is  shown  in  Fig.  10.3  with  the  DH  parameters  of 
Table  10.1  at  arbitrary  posture. 

(a)  Find  the  Jacobian  matrix  of  this  manipulator  at  a  posture  with  axis  Xi 
vertical  and  pointing  downwards,  while  Z2  and  Yi  make  an  angle  of  180°. 
Moreover,  in  this  particular  posture,  Z3  and  Z4  are  vertical  and  pointing 
upwards,  while  Z7  makes  an  angle  of  180°  with  Yi. 

(b)  At  the  posture  described  in  item  (a),  compute  the  joint-rates  that  will 
produce  the  twist 
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(c)  A  wrench  given  by  a  moment  n  and  a  force  f  applied  at  point  P  acts  on 
the  EE  of  the  same  manipulator  at  the  posture  described  in  item  (a)  above. 
Calculate  the  joint  torques  or  moments  required  to  balance  this  wrench, 
which  is  given  by 
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_i_ 

5.10  A  robot  of  the  Puma  type  has  the  architecture  displayed  in  Eig.  4.3,  with  the 
numerical  values  a2  =  0.432  m,  =  0.020  m,  =  0.149  m,  b/[  =  0.432  m. 
Eind  its  maximum  reach  R  as  well  as  the  link  length  a  of  the  manipulator  of 
Eig.  4.17  with  the  same  reach  R. 

5.11  Dialytic  elimination.  The  characteristic  polynomial  of  decoupled  manipula¬ 
tors  for  positioning  tasks  can  be  derived  alternatively  via  dialytic  elimination, 
as  introduced  in  Sect.  5.4.1.  It  is  recalled  here  that  dialytic  elimination  consists 
in  eliminating  one  unknown  from  a  system  of  polynomial  equations  by 
expressing  this  system  in  linear  homogeneous  form,  whereby  each  equation 
is  a  linear  combination  of  various  successive  powers  of  the  unknown  to  be 
eliminated,  including  the  zeroth  power.  This  elimination  can  be  achieved  as 
outlined  below:  In  Eqs.  (4.19a)  and  (4.20a),  express  cos  0i  and  sin  0i  in  terms 
of  tan(0i/2)  =  ti,  thereby  obtaining 

( — A  -\-  C  c^  ~\~  D  t^  -\-  '2.  B  t\  -\-  (C  c^  D  s^  E  Af  0 

{H  +  I  +  J)tl  +  {2G  -  F)h  +  {H  +  I  S3  +  J  +  F)  =  0 

which  can  be  further  expressed  as 

m  ti  -\-  2  B  ti  -\-  n  =0 
p  t^  +  (2  G  —  E)  ti  q  =  0 

with  obvious  definitions  for  coefficients  m,  n,  p,  and  q.  Next,  both  sides  of 
the  two  foregoing  equations  are  multiplied  by  ,  thereby  producing 

mtl  -\-  2  B  t^  -\-  n  ti  =0 
ptl  -\-  {2  G  —  E)  tl  -\-  qti  =0 

Now,  the  last  four  equations  can  be  regarded  as  a  system  of  linear  homoge¬ 
neous  equations,  namely. 


Mti  =  0 
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where  0  is  the  four-dimensional  zero  vector,  while  M  is  a  4  x  4  matrix,  and  ti 
is  a  four-dimensional  vector.  These  arrays  are  defined  as 


M  = 
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Apparently,  ti  ^  0,  and  hence,  M  must  be  singular.  The  characteristic 
polynomial  sought  can  then  be  derived  from  the  condition 

det(M)  =  0 

Show  that  the  last  equation  is  quadratic  in  cos  ^3  and  sin  ^3.  Hence,  the 
foregoing  equation  should  lead  to  a  quartic  equation  in  tan(03/2).  Derive  the 
quartic  equation  involved.  Hint:  Do  not  do  this  by  hand,  for  it  may  be  too 
time-consuming  and  could  quickly  lead  to  algebraic  mistakes.  Use  software 
for  symbolic  computations  instead. 

5.12  Compute  the  workspace  volume  of  the  manipulator  of  Fig.  4.3.  Here,  you 
can  exploit  the  axial  symmetry  of  the  workspace  by  recalling  the  Pappus- 
Guldinus  Theorems — see  any  book  on  multivariable  calculus — that  yield  the 
volume  as  Inq,  with  q  defined  as  the  first  moment  of  the  cross-section,  which 
is  displayed  in  Fig.  5.4b,  with  respect  to  the  axis  of  symmetry,  Zi .  To  this  end, 
you  will  need  the  first  moment  of  a  semicircle  with  respect  to  its  diameter.  This 
information  is  tabulated  in  books  on  elementary  mechanics  or  multivariable 
calculus,  a.k.a.  advanced  calculus. 

5.13  Compute  the  workspace  volume  of  the  manipulator  of  Fig.  4.17,  whose 
workspace  is  sketched  in  Fig.  5.5.  Here,  you  can  also  use  the  Pappus-Guldinus 
Theorem,  as  suggested  in  Exercise  5.12.  However,  the  first  moment  of  the 
cross-section  has  to  be  determined  numerically,  for  the  area  properties  of 
the  curve  that  generates  the  three-dimensional  workspace  are  not  tabulated. 
Now,  for  two  manipulators,  the  Puma-type  and  the  one  under  discussion,  with 
the  same  reach,  determine  which  one  has  the  larger  workspace.  Note:  This 
exercise  is  not  more  difficult  than  others,  but  it  requires  the  use  of  suitable 
software  for  the  calculation  of  area  properties  of  planar  regions  bounded  by 
arbitrary  curves.  Unless  you  have  access  to  such  software,  do  not  attempt  this 
exercise. 

5.14  Show  that  the  maximum  manipul ability  pi  =  ydet(JJ^  )  of  an  orthogonal 
spherical  wrist  is  attained  when  all  three  of  its  axes  are  mutually  orthogonal. 
Find  that  maximum  value. 

5.15  Find  an  expression  for  the  condition  number  of  a  three-revolute  spherical 
wrist  of  twist  angles  0'4  and  as,  and  show  that  this  number  depends  only  on 
0^4,  as,  and  the  intermediate  joint  angle.  Os.  Moreover,  find  values  of  these 
variables  that  minimize  the  condition  number  of  the  manipulator.  Hint:  To 
find  the  required  expression,  the  use  of  the  condition  number  based  on  the 
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Frobenius  norm  is  strongly  recommended.  However,  rendering  the  Jacobian 
matrix  isotropic  can  be  done  by  inspection. 

5.16  Manipulability  of  decoupled  manipulators.  Let  /x^  and  /x^  represent  the 
manipulability  of  the  arm  and  the  wrist  of  a  decoupled  manipulator,  i.e., 


jia  =  Ydet(J2lJ2i),  llw  =  Ydet(Ji2J[2) 

with  Ji2  and  J21  defined  in  Sect.  5.2.  Show  that  the  manipulability  /x  of  the 
overall  manipulator  is  the  product  of  the  two  manipulabilities  given  above, 
i.e.. 


P'  —  Papw 


5.17  Consider  a  planar  two-revolute  manipulator  with  link  lengths  ai  and  02.  Find 
an  expression  of  the  form  K(r,  O2)  for  the  condition  number  of  its  Jacobian, 
with  r  =  02! a\,  and  establish  values  of  r  and  O2  that  minimize  /c,  which 
reaches  a  minimum  value  of  unity. 

5.18  Shown  in  Fig.  5.12  is  an  orthogonal  three-revolute  manipulator  with  an 
isotropic  Jacobian.  Find  the  volume  of  its  workspace.  Now  consider  a  second 
manipulator  with  a  similar  orthogonal  architecture,  but  with  more  common 
dimensions,  i.e.,  with  links  of  equal  length  X.  If  the  two  manipulators  have  the 
same  reach,  that  is,  if 


i±^/ 

2 


find  the  volume  of  the  workspace  of  the  second  manipulator.  Finally,  deter¬ 
mine  the  KCI — see  Sect.  5.8  for  a  definition  of  this  term — of  the  second 
manipulator.  Draw  some  conclusions  with  regard  to  the  performance  of  the 
two  manipulators. 


Chapter  6 

Trajectory  Planning:  Pick-and-Place  Operations 


6.1  Introduction 

The  motions  undergone  by  robotic  mechanical  systems  should  be,  as  a  rule,  as 
smooth  as  possible;  i.e.,  abrupt  changes  in  position,  velocity,  and  acceleration  should 
be  avoided.  Indeed,  abrupt  motions  require  unlimited  amounts  of  power  to  be 
implemented,  which  the  motors  cannot  supply  because  of  their  physical  limitations. 
On  the  other  hand,  abrupt  motion  changes  arise  when  the  robot  collides  with  an 
object,  a  situation  that  should  also  be  avoided.  While  smooth  motions  can  be 
planned  with  simple  techniques,  as  described  below,  these  are  no  guarantees  that  no 
abrupt  motion  changes  will  occur.  In  fact,  if  the  work  environment  is  cluttered  with 
objects,  whether  stationary  or  mobile,  collisions  may  occur.  Under  ideal  conditions, 
a  flexible  manufacturing  cell  is  a  work  environment  in  which  all  objects,  machines 
and  workpieces  alike,  move  with  preprogrammed  motions  that  by  their  nature, 
can  be  predicted  at  any  instant.  Actual  situations,  however,  are  far  from  being 
ideal,  and  system  failures  are  unavoidable.  Unpredictable  situations  should  thus  be 
accounted  for  when  designing  a  robotic  system,  which  can  be  done  by  supplying 
the  system  with  sensors  for  the  automatic  detection  of  unexpected  events  or  by 
providing  for  human  monitoring.  Nevertheless,  robotic  systems  find  applications 
not  only  in  the  well- structured  environments  of  flexible  manufacturing  cells,  but  also 
in  unstructured  environments  such  as  exploration  of  unknown  terrains  and  systems 
in  which  humans  are  present.  The  planning  of  robot  motions  in  the  latter  case  is 
obviously  much  more  challenging  than  in  the  former.  Robot  motion  planning  in 
unstructured  environments  calls  for  techniques  beyond  the  scope  of  those  studied  in 
this  book,  involving  such  areas  as  pattern  recognition  and  artificial  intelligence.  For 
this  reason,  we  have  devoted  this  book  to  the  planning  of  robot  motions  in  structured 
environments  only. 
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Two  typical  tasks  call  for  trajectory  planning  techniques,  namely, 

•  pick-and-place  operations  (PPO),  and 

•  continuous  paths  (CP). 

We  will  study  PPO  in  this  chapter,  with  Chap.  1 1  devoted  to  CP.  Moreover, 
we  will  focus  on  simple  robotic  manipulators  of  the  serial  type,  although  these 
techniques  can  be  directly  applied  to  other,  more  advanced,  robotic  mechanical 
systems. 


6.2  Background  on  PPO 

In  PPO,  a  robotic  manipulator  is  meant  to  take  a  workpiece  from  a  given  initial 
pose,  specified  by  the  position  of  one  of  its  points  and  its  orientation  with  respect  to  a 
certain  coordinate  frame,  to  2i  final  pose,  specified  likewise.  However,  how  the  object 
moves  from  its  initial  to  its  final  pose  is  immaterial,  as  long  as  the  motion  is  smooth 
and  no  collisions  occur.  Pick-and-place  operations  are  executed  in  elementary 
manufacturing  operations  such  as  loading  and  unloading  of  belt  conveyors,  tool 
changes  in  machine  tools,  and  simple  assembly  operations  such  as  putting  roller 
bearings  on  a  shaft.  The  common  denominator  of  these  tasks  is  material  handling, 
which  usually  requires  the  presence  of  conventional  machines  whose  motion  is  very 
simple  and  is  usually  characterized  by  a  uniform  velocity.  In  some  instances,  such 
as  in  packing  operations,  a  set  of  workpieces,  e.g.,  in  a  magazine,  is  to  be  relocated 
in  a  prescribed  pattern  in  a  container,  which  constitutes  an  operation  known  as 
palletizing.  Although  palletizing  is  a  more  elaborate  operation  than  simple  pick- 
and-place,  it  can  be  readily  decomposed  into  a  sequence  of  the  latter  operations. 

It  should  be  noted  that  although  the  initial  and  the  final  poses  in  a  PPO  are 
prescribed  in  the  Cartesian  space,  robot  motions  are  implemented  in  the  joint  space. 
Hence,  the  planning  of  PPO  will  be  conducted  in  the  latter  space,  which  brings 
about  the  need  of  mapping  the  motion  thus  planned  into  the  Cartesian  space,  in 
order  to  ensure  that  the  robot  will  not  collide  with  other  objects  in  its  surroundings. 
The  latter  task  is  far  from  being  that  simple,  since  it  involves  the  rendering  of 
the  motion  of  all  the  moving  links  of  the  robot,  each  of  which  has  a  particular 
geometry.  An  approach  to  path  planning  first  proposed  by  Lozano-Perez  (1981) 
consists  of  mapping  the  obstacles  in  the  joint  space,  thus  producing  obstacles  in  the 
joint  space  in  the  form  of  regions  that  the  joint-space  trajectory  should  avoid.  The 
idea  can  be  readily  implemented  for  simple  planar  motions  and  simple  geometries 
of  the  obstacles.  However,  for  general  3-D  motions  and  arbitrary  geometries,  the 
computational  requirements  make  the  procedure  impractical.  A  more  pragmatic 
approach  would  consist  of  two  steps,  namely,  (a)  planning  a  preliminary  trajectory 
in  the  joint  space,  disregarding  the  obstacles,  and  (b)  visually  verifying  if  collisions 
occur  with  the  aid  of  a  graphics  system  rendering  the  animation  of  the  robot  motion 
in  the  presence  of  obstacles.  The  availability  of  powerful  graphics  hardware  enables 
the  fast  animation  of  robot  motions  within  a  highly  realistic  environment.  Shown  in 
Fig.  6.1  is  a  still  image  of  the  animation  produced  by  RVS,  the  McGill  University 
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Fig.  6.1  Still  image  of  the  animation  of  a  palletizing  operation 


Robot-Visualization  System,  of  the  motion  of  a  robot  performing  a  palletizing 
operation.  Commercial  software  for  robot-motion  rendering  is  available. 

By  inspection  of  the  kinematic  closure  equations  of  robotic  manipulators — see 
Eqs.  (4.5a  and  b) — it  is  apparent  that  in  the  absence  of  singularities,  the  mapping 
of  joint  to  Cartesian  variables,  and  vice  versa,  is  continuous.  Hence,  a  smooth 
trajectory  planned  in  the  joint  space  is  guaranteed  to  be  smooth  in  the  Cartesian 
space,  and  the  other  way  around,  as  long  as  the  trajectory  does  not  encounter  a 
singularity. 

In  order  to  proceed  to  synthesize  the  joint  trajectory,  we  must  then  start  by 
mapping  the  initial  and  final  poses  of  the  workpiece,  which  is  assumed  to  be  rigidly 
attached  to  the  EE  of  the  manipulator,  into  manipulator  configurations  described  in 
the  joint  space.  This  is  readily  done  with  the  methods  described  in  Chap.  4.  Let  the 
vector  of  joint  variables  at  the  initial  and  final  robot  configurations  be  denoted  by  ^  / 
and  0  F ,  respectively.  Moreover,  the  initial  pose  in  the  Cartesian  space  is  defined  by 
the  position  vector  p/  of  the  operation  point  P  of  the  EE  and  a  rotation  matrix  Q/ . 
Likewise,  the  final  pose  in  the  Cartesian  space  is  defined  by  the  position  vector  p/r  of 
P  and  the  rotation  matrix  Qf  .  Moreover,  let  p /  and  p /  denote  the  velocity  and  accel¬ 
eration  of  P ,  while  (o  I  and  o)  j  denote  the  angular  velocity  and  angular  acceleration 
of  the  workpiece,  all  of  these  at  the  initial  pose.  These  variables  at  the  final  pose  are 
denoted  likewise,  with  the  subscript  I  changed  to  F .  Eurthermore,  we  assume  that 
time  is  counted  from  the  initial  pose,  i.e.,  at  this  pose,  ^  =  0.  If  the  operation  takes 
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place  in  time  T ,  then  at  the  final  pose,  t  =  T .  We  have  thus  the  set  of  conditions 
that  define  a  smooth  motion  between  the  initial  and  the  final  poses,  namely. 


p(0) 

=  P/ 

m 

=  0 

p(0) 

=  0 

(6.1a) 

Q(0) 

=  Q/ 

(0(0) 

=  0 

«(0) 

=  0 

(6.1b) 

p(T) 

=  Vf 

piT) 

=  0 

p(r) 

=  0 

(6.1c) 

Q(T) 

=  Qf 

<o{T) 

=  0 

(oiT) 

=  0 

(6.  Id) 

In  the  absence  of  singularities,  then,  the  conditions  of  zero  velocity  and  acceleration 
imply  zero  joint  velocity  and  acceleration,  and  hence, 

0(0)  =  0i  0(0)  =  0  0(0)  =  0  (6.2a) 

0(T)  =  0f  0(T)  =  0  0(T)  =  0  (6.2b) 


6.3  Polynomial  Interpolation 

A  simple  inspection  of  conditions  (6.2a)  and  (6.2b)  reveals  that  a  linear  interpolation 
between  initial  and  final  configurations  will  not  work  here,  and  neither  will  a 
quadratic  interpolation,  for  its  slope  vanishes  only  at  a  single  point.  Hence,  a  higher- 
order  interpolation  is  needed.  On  the  other  hand,  these  conditions  imply,  in  turn, 
six  conditions  for  every  joint  trajectory,  which  means  that  if  a  polynomial  is  to  be 
employed  to  represent  the  motion  of  every  joint,  then  this  polynomial  should  be  at 
least  of  the  fifth  degree.  We  thus  start  by  studying  trajectory  planning  with  the  aid 
of  a  5th-degree  polynomial. 


6.3.1  A  3-4-5  Interpolating  Polynomial 


In  order  to  represent  each  joint  motion,  we  use  here  a  fifth-order  polynomial  s(r), 
namely. 


such  that 


s(x)  =  ar^  +  br^  -h  cr^  -h  dr^  er  f 

0<^<1,  0<r<l 


(6.3) 

(6.4) 


and 


_  t 
^  ~  T 


(6.5) 
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We  will  thus  aim  at  a  normal  polynomial  that,  upon  scaling  both  its  argument  and  the 
polynomial  itself,  will  allow  us  to  represent  each  of  the  joint  variables  Oj  throughout 
its  range  of  motion,  so  that 

Oj  (0  =  e‘  +  (0/  -  e‘ (t)  (6.6a) 

where  and  Oj  are  the  given  initial  and  final  values  of  the  yth  joint  variable.  In 
vector  form,  Eq.  (6.6a)  becomes 

0  (^)  =  0  j  (^0  f  —  0  j^s(x^  (6.6b) 


and  hence, 

0(t)  =  (Of  -  Oi)s'{z)i{t)  =  {Of  -  Oi)l^s'{x)  (6.6c) 

Likewise, 

'Ht)  =  ^{9 F  -  0 i)s" {x)  (6.6d) 

and 

0{t)  =  ^{Of  -Oi)s"'{x)  (6.6e) 

What  we  now  need  are  the  values  of  the  coefficients  of  s{x)  that  appear  in  Eq.  (6.3). 
These  are  readily  found  by  recalling  conditions  (6.2a  and  b),  upon  consideration  of 
Eqs.  (6.6b-d).  We  thus  obtain  the  end  conditions  for  ^(r),  namely, 

^(0)  =  0,  s'(0)  =  0,  ^"(0)  =  0,  ^(1)  =  1,  s'(l)  =  0,  s"(l)  =  0  (6.7) 

The  derivatives  of  ^(r)  appearing  above  are  readily  derived  from  Eq.  (6.3),  i.e., 

^'(r)  =  5ar^  +  4br^  +  3cr^  +  2dr  +  e  (6.8) 

and 

^"(r)  =  20ar^  +  Ubr^  +  6cr  +  2d  (6.9) 

Thus,  the  first  three  conditions  of  Eq.  (6.7)  lead  to 


/  =  e  =  d  =  0 


(6.10) 
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Fig.  6.2  3-4-5  interpolation  polynomial  and  its  derivatives 


while  the  last  three  conditions  yield  three  linear  equations  m  a,  b,  and  c,  namely, 

Cl  b  c  —  1  (6.11a) 

5(3  +  4/?  +  3c  =  0  (6.11b) 

20(3  +  I2b  +  6c  =  0  (6.11c) 

Upon  solving  the  three  foregoing  equations  for  the  three  aforementioned  unknowns, 
we  obtain 


a  =  6,  b  =  —15,  c  =  10  (6.12) 

and  hence,  the  normal  polynomial  sought  is 

s(z)  =  6r^  -  15r^  +  lOC  (6.13) 

which  is  called  a  3-4-5  polynomial. 

This  polynomial  and  its  first  three  derivatives,  all  normalized  to  fall  within  the 
(—1,1)  range,  are  shown  in  Fig.  6.2.  Note  that  the  smoothness  conditions  imposed  at 
the  outset  are  respected  and  that  the  curve  thus  obtained  is  a  monotonically  growing 
function  of  r,  a  rather  convenient  property  for  the  problem  at  hand. 

It  is  thus  possible  to  determine  the  evolution  of  each  joint  variable  if  we  know 
both  its  end  values  and  the  time  T  required  to  complete  the  motion.  If  no  extra 
conditions  are  imposed,  we  then  have  the  freedom  to  perform  the  desired  motion 
in  as  short  a  time  T  as  possible.  Note,  however,  that  this  time  cannot  be  given  an 
arbitrarily  small  value,  for  we  must  respect  the  motor  specifications  on  maximum 
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velocity  and  maximum  torque,  the  latter  being  the  subject  of  Chap.  7.  In  order  to 
ease  the  discussion,  we  limit  ourselves  to  specifications  of  maximum  joint  velocity 
and  acceleration  rather  than  maximum  torque.  From  the  form  of  function  Oj{t) 
of  Eq.  (6.6a),  it  is  apparent  that  this  function  takes  on  extreme  values  at  points 
corresponding  to  those  at  which  the  normal  polynomial  attains  its  extrema.  In  order 
to  find  the  values  of  r  at  which  the  first  and  second  derivatives  of  ^(r)  attain 
maximum  values,  we  need  to  zero  its  second  and  third  derivatives.  These  derivatives 
are  displayed  below: 


s'(t)  =  -  60t^  +  30t^  (6.14a) 

/'(t)  =  120r^  -  180t^  +  60r  (6.14b) 

s'"(t)  =  360r2  -  360r  +  60  (6.14c) 

from  which  it  is  apparent  that  the  second  derivative  vanishes  at  the  two  ends  of  the 
interval  0  <  r  <  1 .  Additionally,  the  same  derivative  vanishes  at  the  midpoint  of  the 
same  interval,  i.e.,  at  r  =  1/2.  Hence,  the  maximum  value  of  ^'(r),  is  readily 
found  as 


max 


(6.15) 


and  hence,  the  maximum  value  of  the  /  th  joint  rate  takes  on  the  value 

15(0f-0p 

(6.16) 

8T 

which  becomes  negative,  and  hence,  a  local  minimum,  if  the  difference  in  the 
numerator  is  negative.  The  values  of  r  at  which  the  second  derivative  attains 
its  extreme  values  are  likewise  determined.  The  third  derivative  vanishes  at  two 
intermediate  points  ri  and  T2  of  the  interval  0  <  r  <  1 ,  namely,  at 


1 

H  2  —  “  i  - 

2  6 


(6.17) 


and  hence,  the  maximum  value  of  ^''(r)  is  readily  found  as 


s''  =  s"  I  -  - 

^max  ^  I  2  5 


1  lOv^ 


(6.18) 


while  the  minimum  is  given  as 


1  V^' 


lOv^ 


(6.19) 
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Therefore,  the  maximum  value  of  the  joint  acceleration  is  as  shown  below: 


Likewise, 


max  — 


10 {Of  -  0j) 

~  72 


and  hence. 


*max 


=  i'"(0)  =  =  60 


///  , 


(0/)max  =  60 


T3 


(6.20) 


(6.21) 


Thus,  Eqs.  (6.16)  and  (6.20)  allow  us  to  determine  T  for  each  joint  so  that  the  joint 
rates  and  accelerations  lie  within  the  allowed  limits.  Obviously,  since  the  motors  of 
different  joints  are  different,  the  minimum  values  of  T  allowed  by  the  joints  will  be, 
in  general,  different.  Of  those  various  values  of  T,  we  will,  of  course,  choose  the 
largest  one. 


6.3.2  A  4-5-6-7  Interpolating  Polynomial 

Now,  from  Eq.  (6.14c),  it  is  apparent  that  the  third  derivative  of  the  normal 
polynomial  does  not  vanish  at  the  end  points  of  the  interval  of  interest.  This  implies 
that  the  third  time  derivative  of  Oj  (t),  also  known  as  the  joint  Jerk,  does  not  vanish 
at  those  ends  either.  It  is  desirable  to  have  this  derivative  as  smooth  as  the  first  two, 
but  this  requires  us  to  increase  the  order  of  the  normal  polynomial.  In  order  to  attain 


the  desired  smoothness,  we  will  then  impose  two  more  conditions,  namely, 

^'"(0)  =  0,  ^'"(1)  =  0  (6.22) 

We  now  have  eight  conditions  on  the  normal  polynomial,  which  means  that  the 
polynomial  degree  should  be  increased  to  seven,  namely, 

iS'(t)  =  ciT^  “h  bx^  -\-  cx^  “h  dx^  ex^  f  x^  “h  gx  “h  h  (6.23a) 

whose  derivatives  are  readily  determined  as  shown  below: 

^'(r)  =  lax^  +  6bx^  +  5cx^  4dx^  3ex^  +  2/r  +  g  (6.23b) 
^"(r)  =  42ax^  +  SObx"^  +  20cr^  +  Udx^  +  6ex  +  2/  (6.23c) 

^"'(r)  =  210(21^  +  UObx^  +  60cr^  +  24dx  +  6e  (6.23d) 
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Fig.  6.3  4-5-6-7  interpolating  polynomial  and  its  derivatives 


The  first  three  conditions  of  Eq.  (6.7)  and  the  first  condition  of  Eq.  (6.22)  readily 
lead  to 


e  =  f  =g  =  h  =  0  (6.24) 

Eurthermore,  the  last  three  conditions  of  Eq.  (6.7)  and  the  second  condition  of 
Eq.  (6.22)  lead  to  four  linear  equations  in  four  unknowns,  namely, 

G  b  c  d  —  1  (6.25a) 

“h  Gb  -\-  5c  “h  4-ci  =  0  (6.25b) 

42t3  “h  30b  “h  20c  “h  12cf  =  0  (6.25c) 

210^  +  I20b  +  60c  -h24d  =0  (6.25d) 

and  hence,  we  obtain  the  solution 

a  =  -20,  b  =  70,  c  =  -84,  d  =  35  (6.26) 

the  desired  polynomial  thus  being 

s(t)  =  -20C  +  70C  -  84t®  +  35r^  (6.27) 


which  is  a  4-5 -6-7  polynomial.  This  polynomial  and  its  first  three  derivatives, 
normalized  to  fall  within  the  range  (—1, 1),  are  plotted  in  Eig.  6.3.  Note  that  the 
4-5-6-7  polynomial  is  similar  to  that  of  Eig.  6.2,  except  that  the  third  derivative  of 
the  former  vanishes  at  the  extremes  of  the  interval  of  interest.  As  we  will  presently 
show,  this  smoothness  has  been  obtained  at  the  expense  of  higher  maximum  values 
of  the  first  and  second  derivatives. 
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We  now  determine  the  maximum  values  of  the  velocity  and  acceleration 
produced  with  this  motion.  To  this  end,  we  display  below  the  first  three  derivatives, 
namely. 


5'(r)  =  -140t®  +  420t^  -  420r'‘  +  UOz^  (6.28a) 

i"(r)  =  -840t^  +  2100r'‘  -  1680t^  +  420t^  (6.28b) 

s'"iT)  =  -4200r'‘  +  8400r^  -  5040r^  +  840t  (6.28c) 

The  first  derivative  attains  its  extreme  values  at  points  where  the  second  derivative 
vanishes.  Upon  zeroing  the  latter,  we  obtain 

-  4r  +  1)  =  0  (6.29) 


which  clearly  contains  a  double  root  at  r  =  0.  Moreover,  the  cubic  polynomial 
in  the  parentheses  above  admits  one  real  root,  namely,  r  =  1/2,  which  yields  the 
maximum  value  of  ^'(r),  i.e.. 


max 


35 

16 


whence  the  maximum  value  of  the  yth  joint  rate  is  found  as 


max  — 


35(0/ -0/) 
167 


(6.30) 


(6.31) 


Likewise,  the  points  of  maximum  joint  acceleration  are  found  upon  zeroing  the 
third  derivative  of  ^(r),  namely. 


=  -4200r'‘  +  8400r^  -  5040r2  +  840r  =  0  (6.32) 


or 


r(T-l)(5T^-5T+ 1)  =  0  (6.33) 

which  yields,  in  addition  to  the  two  end  points,  two  intermediate  extreme  points, 
namely. 


1  Vs 

U  2  —  “  - 

’  2  10 

and  hence,  the  maximum  value  of  acceleration  is  found  to  be 

84^5 


(6.34) 


"^max  ^  (^l) 


25 


(6.35) 
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the  minimum  occurring  at  r  =  r2,  with  maximum  value  of  the 

7  th  joint  acceleration  is  thus 


max  — 


84^/5 

25 


(6.36) 


which  becomes  a  minimum  if  the  difference  in  the  numerator  is  negative.  Likewise, 
the  zeroing  of  the  fourth  derivative  leads  to 

-20r^  +  30t^  -  12r  +  1  =  0 


whose  three  roots  are 


and  hence, 


\-y/V5 


1 

T2--, 


1  +  yffjs 
2 


s"'  =  s'" 

max 


=  42,  <  =/"(0.5) 


///, 


105 


i.e., 

max{|5'"(T)|}  =  ^  =s'l^  (6.37) 

2 

As  in  the  case  of  the  fifth-order  polynomial,  it  is  possible  to  use  the  foregoing 
relations  to  determine  the  minimum  time  T  during  which  it  is  possible  to  perform  a 
given  PPO  while  observing  the  physical  limitations  of  the  motors. 


6.4  Cycloidal  Motion 

An  alternative  motion  that  produces  zero  velocity  and  acceleration  at  the  ends  of  a 
finite  interval  is  the  cycloidal  motion.  In  normal  form,  this  motion  is  given  by 

^(r)  =  r - sin27rr 

In 

its  derivatives  being  readily  derived  as 

s'{x)  =  1  —  cos27rr 
=  In  sin27rr 
s"'{x)  =  47r^cos27rr 


(6.38a) 

(6.38b) 

(6.38c) 

(6.38d) 
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Fig.  6.4  The  normal  cycloidal  motion  and  its  time  derivatives 


The  cycloidal  motion  and  its  first  three  time-derivatives,  normalized  to  fall  within 
the  range  (—1,1),  are  shown  in  Fig.  6.4.  Note  that  while  this  motion,  indeed,  has  zero 
velocity  and  acceleration  at  the  ends  of  the  interval  0  <  r  <  1,  its  jerk  is  nonzero  at 
these  points  and  hence,  exhibits  jump  discontinuities  at  the  ends  of  that  interval. 
When  implementing  the  cycloidal  motion  in  PPO,  we  have,  for  the  yth  joint, 

ej{t)  =  e]  +  {9^  -e‘)s{x)  (6.39a) 

qF  _  qI 

9j(t)  =  ^  ^  ^  s' (x)  (6.39b) 

T 

qF  _  qI 

9j(t)  =  ^^s"(x)  (6.39c) 

Moreover,  as  the  reader  can  readily  verify,  under  the  assumption  that  9^  >  9’  ,  this 
motion  attains  its  maximum  velocity  at  the  center  of  the  interval,  i.e.,  at  r  =  0.5, 
the  maximum  being 


4ax  =  ‘^'(0-5)  =  2 

and  hence, 

(0y)max  =  (6.40a) 

Likewise,  the  jth  joint  acceleration  attains  its  maximum  and  minimum  values  at 
r  =  0.25  and  r  =  0.75,  respectively,  i.e.. 


=  ^"(0.25)  =  ^"(0.75)  =  27r 


(6.40b) 
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and  hence, 


{Oj )max  =  ^ (0/  -0]),  {Oj )„i„  =  - ^ (0/  -  e] )  (6.40c) 

Moreover,  attains  its  extrema  at  the  ends  of  the  interval,  i.e., 

Cax  =  ^'"(0)  =  ^'"(1)  =  4;r2  (6.41) 

and  hence, 

Ajr'2 

idjU..  =  ^ieJ  -e])  (6.42) 

Thus,  if  motion  is  constrained  by  the  maximum  speed  delivered  by  the  motors, 
the  minimum  time  Tj  for  the  y  th  joint  to  produce  the  given  PPO  can  be  readily 
determined  from  Eq.  (6.40a)  as 


iOj) 


j  /max 


(6.43) 


and  hence,  the  minimum  time  in  which  the  operation  can  take  place  can  be  readily 
found  as 


Tmin  =  2  max  <  (6.44) 

^  (  (^/Omax  ] 

If  joint-acceleration  constraints  are  imposed,  then  a  similar  procedure  can  be 
followed  to  find  the  minimum  time  in  which  the  operation  can  be  realized.  As  a 
matter  of  fact,  rather  than  maximum  joint  accelerations,  maximum  joint  torques  are 
to  be  respected.  How  to  determine  these  torques  is  studied  in  detail  in  Chap.  7. 


6.5  Trajectories  with  via  Poses 

The  polynomial  trajectories  discussed  above  do  not  allow  the  specification  of 
intermediate  Cartesian  poses  of  the  EE.  All  they  guarantee  is  that  the  Cartesian 
trajectories  prescribed  at  the  initial  and  final  instants  are  met.  One  way  of  verifying 
the  feasibility  of  the  Cartesian  trajectories  thus  synthesized  was  outlined  above 
and  consists  of  using  a  graphics  system,  preferably  with  animation  capabilities,  to 
produce  an  animated  rendering  of  the  robot  motion,  thereby  allowing  for  verification 
of  collisions.  If  the  latter  occur,  we  can  either  try  alternative  branches  of  the  inverse 
kinematics  solutions  computed  at  the  end  poses  or  modify  the  trajectory  so  as  to 
eliminate  collisions.  We  discuss  below  the  second  approach.  This  is  done  with  what 
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are  called  via  poses,  i.e.,  poses  of  the  EE  in  the  Cartesian  space  that  lie  between  the 
initial  and  the  final  poses,  and  are  determined  so  as  to  avoid  collisions  (Gosselin  and 
Hadj-Messaoud  1993).  Eor  example,  if  upon  approaching  the  final  pose  of  the  PPO, 
the  manipulator  is  detected  to  interfere  with  the  surface  on  which  the  workpiece  is 
to  be  placed,  a  via  pose  is  selected  close  to  the  final  point  so  that  at  this  pose,  the 
workpiece  is  far  enough  from  the  surface.  Prom  inverse  kinematics,  values  of  the 
joint  variables  can  be  determined  that  correspond  to  the  aforementioned  via  poses. 
These  values  can  now  be  regarded  as  points  on  the  joint- space  trajectory  and  are 
hence  called  via  points.  Obviously,  upon  plotting  each  joint  variable  vs.  time,  via 
points  appear  as  points  on  those  plots  as  well. 

The  introduction  of  via  points  in  the  joint- space  trajectories  amounts  to  an 
increase  in  the  number  of  conditions  to  be  satisfied  by  the  desired  trajectory.  Eor 
example,  in  the  case  of  the  polynomial  trajectory  synthesized  for  continuity  up  to 
second  derivatives,  we  can  introduce  two  via  points  by  requiring  that 


^(ti)  =  ^1,  s{t2)  =  ^2  (6.45) 

where  x\,X2,si,  and  S2  depend  on  the  via  poses  prescribed  and  the  instants  at  which 
these  poses  are  desired  to  occur.  Hence,  and  ^2  differ  from  joint  to  joint,  although 
the  occurrence  instants  x\  and  X2  are  the  same  for  all  joints.  Thus,  we  will  have  to 
determine  one  normal  polynomial  for  each  joint.  Purthermore,  the  ordinate  values 
and  ^2  of  the  normal  polynomial  at  via  points  are  determined  from  the  corresponding 
values  of  the  joint  variable  determined,  in  turn,  from  given  via  poses  through  inverse 
kinematics.  Once  the  via  values  of  the  joint  variables  are  known,  the  ordinate  values 
of  the  via  points  of  the  normal  polynomial  are  found  from  Eq.  (6.6a).  Since  we 
have  now  eight  conditions  to  satisfy,  namely,  the  six  conditions  (6.7)  plus  the  two 
conditions  (6.45),  we  need  a  seventh-order  polynomial,  i.e., 

iS'(t)  =  ax^  “h  hx^  -\-  cx^  “h  dx^  ex^  fx^  gx  h  (6.46) 

Again,  the  first  three  conditions  of  Eq.  (6.7)  lead  to  the  vanishing  of  the  last  three 
coefficients,  i.e.. 


f  =  g  =  h  =  0  (6.47) 

Purther,  the  five  remaining  conditions  are  now  introduced,  which  leads  to  a  system 


of  five  linear  equations  in  five  unknowns,  namely, 

a-\-b-\-c-\-d-\-e  —  1  (6.48a) 

la  “h  Gb  “h  Sc  “h  A^d  Se  =  0  (6.48b) 

42^3  “h  sob  20c  “h  Hd  6c  =  0  (6.48c) 

xja  +  rfZ?  +  rfc  +  rfc?  +  r^^c  =  (6.48d) 

xja  +  x^b  +  r|c  +  x^d  +  r|c  =  ^2  (6.48e) 
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where  ri,  r2,  and  ^2  are  all  data.  For  example,  if  the  via  poses  occur  at  10  and 
90%  of  r,  we  have 


n  =  1/10,  r2  =  9/10  (6.48f) 

the  polynomial  coefficients  being  found  as 

a  =  100(12286  +  12500^1  -  12500^2)/729  (6.49a) 

b  =  100(-38001  -  48750^1  +  38750^2)/729  (6.49b) 

c  =  (1344358  +  2375000^1  -  1375000^2)/243  (6.49c) 

d  =  (-1582435  -  4625000^1  +  1625000^2)/729  (6.49d) 

e  =  10(12159  +  112500^1  -  12500^2)7729  (6.49e) 


The  shape  of  each  joint  trajectory  thus  depends  on  the  values  of  and  ^2  found 
from  Eq.  (6.6a)  for  that  trajectory. 


6.6  Synthesis  of  PPO  Using  Cubic  Splines 

When  the  number  of  via  poses  increases,  the  foregoing  approach  may  become 
impractical,  or  even  unreliable.  Indeed,  forcing  a  trajectory  to  pass  through  a 
number  of  via  points  and  meet  endpoint  conditions  is  equivalent  to  interpolation. 
We  have  seen  that  an  increase  in  the  number  of  conditions  to  be  met  by  the  normal 
polynomial  amounts  to  an  increase  in  the  degree  of  this  polynomial.  Now,  finding 
the  coefficients  of  the  interpolating  polynomial  requires  solving  a  system  of  linear 
equations.  As  we  saw  in  Sect.  5.8,  the  computed  solution,  when  solving  a  system 
of  linear  equations,  is  corrupted  with  a  relative  roundoff  error  that  is  roughly  equal 
to  the  relative  roundoff  error  of  the  data  multiplied  by  an  amplification  factor  that 
is  known  as  the  condition  number  of  the  system  matrix.  As  we  increase  the  order 
of  the  interpolating  polynomial,  the  associated  condition  number  rapidly  increases, 
a  fact  that  numerical  analysts  discovered  some  time  ago  (Kahaner  et  al.  1989). 
In  order  to  cope  with  this  problem,  orthogonal  polynomials,  such  as  those  bearing 
the  names  of  Chebyshev,  Laguerre,  Legendre,  and  so  on,  have  been  proposed.  While 
orthogonal  polynomials  alleviate  the  problem  of  a  large  condition  number,  they  do 
this  only  up  to  a  certain  extent.  As  an  alternative  to  higher-order  polynomials,  spline 
functions  have  been  found  to  offer  more  robust  interpolation  schemes  (Dierckx 
1993).  Spline  functions,  or  splines,  for  brevity,  are  piecewise  polynomials  with 
continuity  properties  imposed  at  the  supporting  points.  The  latter  are  those  points  at 
which  two  neighboring  polynomials  join. 

The  attractive  feature  of  splines  is  that  they  are  defined  as  a  set  of  rather  lower- 
degree  polynomials  joined  at  a  number  of  supporting  points.  Moreover,  the  matrices 
that  arise  from  an  interpolation  problem  associated  with  a  spline  function  are  such 


270 


6  Trajectory  Planning:  Pick-and-Place  Operations 


that  their  condition  number  is  only  slightly  dependent  on  the  number  of  supporting 
points,  and  hence,  splines  offer  the  possibility  of  interpolating  over  a  virtually 
unlimited  number  of  points  without  producing  serious  numerical  conditioning 
problems. 

Below  we  expand  on  periodic  cubic  splines,  for  these  will  be  shown  to  be 
specially  suited  for  path  planning  in  robotics. 

A  cubic  spline  function  ^(x)  connecting  N  points  (x^,  yk),  for  k  = 
1, 2, . . . ,  A,  is  3.  function  defined  piecewise  by  A  —  1  cubic  polynomials  joined  at 
the  points  Pk,  such  that  ^(x^)  =  Furthermore,  the  spline  function  thus  defined 
is  twice  differentiable  everywhere  in  xi  <  x  <  x^.  Hence,  cubic  splines  are  said  to 
be  functions,  i.e.,  to  have  continuous  derivatives  up  to  the  second  order. 

Cubic  splines  are  optimal  in  the  sense  that  they  minimize  a.  functional,  i.e.,  an 
integral  defined  as 


(x)  dx 


subject  to  the  constraints 


^(x^)  =  yk,  k  = 

where  Xk  and  yk  are  given.  The  aforementioned  optimality  property  has  a  simple 
kinematic  interpretation:  Among  all  functions  defining  a  motion  so  that  the  plot  of 
this  function  passes  through  a  set  of  points  F*i(xi,  ^i),  P2(x2,  S2), . . . ,  Pn(^n^  ^n) 
in  the  x-^  plane,  the  cubic  spline  is  the  one  containing  the  minimum  acceleration 
magnitude.  In  fact,  F ,  as  given  above,  is  the  square  of  the  Euclidean  norm  (Halmos 
1974)  of  ^''(x),  i.e.,  F  turns  out  to  be  a  measure  of  the  magnitude  of  the  acceleration 
of  a  displacement  program  given  by  ^(x),  if  we  interpret  ^  as  displacement  and  x  as 
time. 

Let  Pk(xk,  yk)  and  Pk-\-i(xk-\-i,  yk-\-i)  be  two  consecutive  supporting  points. 
The  kth  cubic  polynomial  Sk  (x)  between  those  points  is  assumed  to  be  given  by 

=  A/;  (x  -  Xkf  +  Bkix  -  Xkf  +  Q  (x  -  Xk)  +  Dk  (6.50a) 

for  x^  <  X  <  x^+i.  Thus,  for  the  spline  ^(x),  4(A  —  1)  coefficients  Ak,  Bk,  Ck,  Dk, 
fork  =  1 , . . . ,  A  —  1 ,  are  to  be  determined.  These  coefficients  will  be  computed 
presently  in  terms  of  the  given  function  values  {sk  and  the  second  derivatives  of 
the  spline  at  the  supporting  points,  {s'j^(xk)}i  ,  as  explained  below: 

We  will  need  the  first  and  second  derivatives  of  Sk  (x)  as  given  above,  namely. 


4(x)  =  3^i;(x  -  Xk)^  +  2Bk(x  -  Xk)  +  Ck 
s'^(x)  =  6Akix  -  Xk)  +  2Bk 


(6.50b) 

(6.50c) 
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whence  the  relations  below  follow  immediately: 


a  =  4 


(6.51a) 

(6.51b) 

(6.51c) 


where  we  have  used  the  abbreviations 

Sk  =  s{xk),  4  =  4'  =  (6.52) 


Furthermore,  let 


=  Xk+i  -Xk  (6.53) 

From  the  above  relations,  we  have  expressions  for  coefficients  and  in  terms 
of  s'j^  and  Sk ,  respectively,  but  the  expression  for  Ck  is  given  in  terms  of  s'j^ .  What 
we  would  like  to  have  are  similar  expressions  for  and  Q,  i.e.,  in  terms  of  Sk 
and  s'j^.  The  relations  sought  will  be  found  by  imposing  the  continuity  conditions 
on  the  spline  function  and  its  first  and  second  derivatives  with  respect  to  x  at  the 
supporting  points.  These  conditions  are,  then,  for  k  =  1,  2,  . . . ,  A  —  1, 


=  ^^+1  (6.54a) 

Sk(xk+i)  =  4+1  (6.54b) 

4fe+i)  =  4+1  (6.54c) 


Upon  substituting  4(x^+i),  as  given  by  Eq.  (6.50c),  into  Eq.  (6.54c),  we  obtain 


6AkAxk  +  2Bk  =  2Bk+i 


but  from  Eq.  (6.51a),  we  have  already  an  expression  for  B^,  and  hence,  one  for 
Bk+i  as  well.  Substituting  these  two  expressions  in  the  above  equation,  we  obtain 
an  expression  for  y4^,  namely. 


(6.54d) 


Furthermore,  if  we  substitute  s^ix^+i),  as  given  by  Eq.  (6.50a),  into  Eq.  (6.54a),  we 
obtain 


Aki^Axk^  ~\~  Bj^i^Axj^^  -\-  Cj^Axj^  Dj^  —  ^/:+i 
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But  we  already  have  values  for  and  from  Eqs.  (6.54d),  (6.51a), 

and  (6.51c),  respectively.  Upon  substituting  these  values  in  the  foregoing  equation, 
we  obtain  the  desired  expression  for  in  terms  of  function  and  second-derivative 
values,  i.e.. 


Ck  -  ^  Axfc  (4Vi  +  24) 


(6.54e) 


In  summary,  then,  we  now  have  expressions  for  all  four  coefficients  of  the  k\h 
polynomial  in  terms  of  function  and  second-derivative  values  at  the  supporting 
points,  namely. 


with 


Bk  =  \  4  (6-55b) 

Q  =  ^  -  i  Axfc  (4+1  +  24)  (6.55c) 

iXXk  D 

Dk  =  Sk  (6.55d) 


Ask  =  Sk+i  -  Sk  (6.55e) 

Therefore,  in  order  to  find  the  above  coefficients,  all  we  need  is  the  set  of 
values  of  the  second  derivatives  {s'j^}f  at  the  supporting  points.  To  compute  these 
values,  we  impose  the  continuity  condition  on  the  first  derivative,  Eq.  (6.54b),  after 
substitution  of  Eq.  (6.50b),  which  yields 

3^^(Ax^)^  +  IBkAxk  Ck  =  Q+i 
or,  if  we  shift  to  the  previous  polynomial. 


+  '^Bk-iAxk-i  +  Ck-i  —  Ck 

Now,  if  we  substitute  expressions  (6.55a-c)  in  the  above  equation,  a  linear  system 
of  N  —2  simultaneous  equations  for  the  N  unknowns  {s'j^}f  is  obtained,  namely, 

(A  x^)4+i  +  2(Av^_i  +  Ax^)4  +  (Av^_i)4_i 
^  /  Ask  Ask-i 


Axk  Axk-i 


for  k  =  2, . . . ,  N  —  I  . 


(6.56) 
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Further,  let  s  be  the  -dimensional  vector  whose  ki\\  component  is  Sk,  with 
vector  s"  being  defined  likewise,  i.e., 

s  =  [si,--- ,SNf  ,  s"  =  [//,•••  (6.57) 

The  relationship  between  s  and  s"  of  Eq.  (6.56)  can  then  be  written  in  vector 
form  as 


As"  =  6Cs 


where  A  and  C  are  (N  —2)  x  N  matrices  defined  as: 


Oil  20:1,2  0:2  0 

0  0:2  20:2,3  0i3 


0  O' 

0  0 


0  0  ...  oi]\i'"  01  0 

0  0  0  •••  oifsjff  2oi]^"^]^'  oi]^' 


and 


Pi  —Pl,2  Pi  0 

0  P2  —Pl,!  Pi 


0  O' 

0  0 


0  0 
0  0 


0 


Pn'"  —pN'",N"  Pn"  0 
Pn"  —Pn",N'  Pn'- 


while  for  i,  j,k  =  1 , . . . ,  A  —  1 , 


Oik  —  Ax/^,  oifj  —  Oil  oi j  f 

pk  =  l/oik,  PiJ  =  Pi  +  Pj 


N'  =  N-l,  N"  =  N-2,  N"'  =  N-3 


(6.58a) 


(6.58b) 


(6.58c) 


(6.58d) 

(6.58e) 


(6.58f) 


Thus,  two  additional  equations  are  needed  to  render  Eq.  (6.58a)  a  determined 
system.  The  additional  equations  are  derived,  in  turn,  depending  upon  the  class  of 
functions  one  is  dealing  with,  which  thus  gives  rise  to  various  types  of  splines.  For 
example,  if  s'^  and  s'^  are  defined  as  zero,  then  one  obtains  natural  cubic  splines, 
the  name  arising  by  an  analogy  with  beam  analysis.  Indeed,  in  beam  theory,  the 
boundary  conditions  of  a  simply-supported  beam  establish  the  vanishing  of  the 
bending  moments  at  the  ends.  From  beam  theory,  moreover,  the  bending  moment 
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is  proportional  to  the  second  derivative  of  the  elastica,  or  neutral  axis,  of  the  beam 
with  respect  to  the  abscissa  along  the  beam  axis  in  the  undeformed  configuration. 
In  this  case,  vector  s"  becomes  of  dimension  N  —2,  and  hence,  matrix  A  becomes, 
correspondingly,  of  (N  —  2)  x  (N  —  2),  namely. 


2a  I  ^2  Oil  0 

a2  2^2,3  «3 


0 

0 


0  ...  ajsf  tn 

0  0  •••  oi^^"  2a^" 


(6.59) 


On  the  other  hand,  if  one  is  interested  in  periodic  functions,  which  is  often  the 
case  when  synthesizing  pick-and-place  motions,  then  the  conditions  =  s 2^,  s\  — 
s'^,  s'l  =  s'!^  are  imposed,  thereby  producing  periodic  cubic  splines.  The  last  of 
these  conditions  is  used  to  eliminate  one  unknown  in  Eq.  (6.58a),  while  the  second 
condition,  namely  the  continuity  of  the  first  derivative,  is  used  to  add  an  equation. 
We  have,  then. 


=  4  (6.60) 

which  can  be  written,  using  Eq.  (6.54b),  as 

^5  =  (6-61) 

Upon  substituting  s'j^_^(xn),  as  given  by  Eq.  (6.50b),  into  the  above  equation, 
we  obtain 

=  3A]\[-iAx^_i  +  2B]\[-iAx]\[-i  +  C^y-i  (6.62) 

Now  we  use  Eqs.  (6.55a-c)  and  simplify  the  expression  thus  resulting,  which 
leads  to 


2(^Axi  4“  Ax ]\[ —i^s ^  4“  AX1S2  4“  Ax]\[—is —  6  f - — - j  (6.63) 

yAxi  Axj\^—iJ 

thereby  obtaining  the  last  equation  required  to  solve  the  system  of  equations  given 
by  Eqs.  (6.58a-c).  We  thus  have  (N  —  1)  independent  equations  to  solve  for 
(N  —  1)  unknowns,  namely,  for  k  =  1, . . . ,  A  —  1,  being  equal  to 
Expressions  for  matrices  A  and  C,  as  applicable  to  periodic  cubic  splines,  are  given 
in  Eqs.  (11.59a  and  b). 

While  we  focused  in  the  above  discussion  on  cubic  splines,  other  types  of  splines 
could  have  been  used.  Eor  example,  Thompson  and  Patel  (1987)  used  B-splines  in 
robotics  trajectory  planning. 
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Example  6.6.1  (Approximation  of  a  4-5-6-7  Polynomial  with  a  Cubic  Spline).  Find 
the  cubic  spline  that  interpolates  the  4-5-6-7  polynomial  of  Fig.  6.3  with  N  I 
equally-spaced  supporting  points  and  plot  the  interpolation  error  for  =  3  and 
N  =  10. 


Solution:  Let  us  use  a  natural  spline,  in  which  case  the  second  derivative  at  the  end 
points  vanishes,  with  vector  s"  thus  losing  two  components.  That  is,  we  now  have 
only  N  —2  unknowns  {  to  determine.  Correspondingly,  matrix  A  then  loses 

its  first  and  last  columns  and  hence,  becomes  a  square  (N  —2)  x  (N  —  2)  matrix. 
Moreover, 


Axk 


1 

A’ 


and  matrices  A  and  C  become,  correspondingly. 


"4  1  0  •  •  •  0" 
14  1  •••  0 

•  .  •  •  • 

•  •  •  •  . 

0  ...  1  41 

0  0  •••  1  4 


and 


"1 

-2  1 

0 

• 

• 

• 

o 

o 

0 

1  -2 

• 

1 

•  • 

• 

• 

•  o 

•  o 

0 

• 

0 

1 

• 

-2  1  0 

_0 

0  0 

•  •  • 

1  -2  1_ 

the  vector  of  second  derivatives  at  the  supporting  points,  s",  then  being  readily 
obtained  as 


s"  =  6A“‘Cs 

With  the  values  of  the  second  derivatives  at  the  supporting  points  known,  the 
calculation  of  the  spline  coefficients  Ck,  and  for  /:  =  1, . . . ,  A,  is  now 

straightforward.  Let  the  interpolation  error,  e(x),  be  defined  as  e(x)  =  ^(x)  —  p(x), 
where  ^(x)  is  the  interpolating  spline  and  p(x)  is  the  given  polynomial.  This  error 
and  its  derivatives  e'(x),  e"(x),  and  e"'(x)  are  plotted  in  Figs.  6.5  and  6.6  for  A  =  3 
and  A  =  10,  respectively.  What  we  observe  is  an  increase  of  more  than  one  order 
of  magnitude  in  the  error  as  we  increase  the  order  of  the  derivative  by  one.  Thus, 
the  order  of  magnitude  of  acceleration  errors  is  usually  higher  than  two  orders  of 
magnitude  above  the  displacement  errors,  a  fact  that  should  not  be  overlooked  in 
applications. 
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Fig.  6.5  Errors  in  the  approximation  of  a  4-5-6-7  polynomial  with  a  natural  cubic  spline,  using 
four  supporting  points 
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Fig.  6.6  Errors  in  the  approximation  of  a  4-5-6-7  polynomial  with  a  natural  cubic  spline,  using 
eleven  supporting  points 
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6.7  Exercises 

6.1  A  common  joint-rate  program  for  pick-and-place  operations  is  the  trapezoidal 
profile  of  Fig.  6.7,  whereby  we  plot  s' (r)YS.r,  using  the  notation  introduced  in 
Chap.  7,  i.e.,  with  ^(r)  and  r  defined  as  dimensionless  variables.  Here,  ^'(r) 
starts  and  ends  at  0.  From  its  start  to  a  value  ri,  ^'(r)  grows  linearly,  until 
reaching  a  maximum  then,  this  function  remains  constant  until  a  value 
r2  is  reached,  after  which  the  function  decreases  linearly  to  zero  at  the  end  of 
the  interval. 

Clearly,  this  profile  has  a  discontinuous  acceleration  and  hence,  is  bound 
to  produce  shock  and  vibration.  However,  the  profile  can  be  smoothed  with  a 
spline  interpolation  as  indicated  below. 

(a)  Find  the  value  of  in  terms  of  ri  and  r2  so  that  ^(0)  =  0  and  ^(1)  =  1. 

(b)  Plot  ^  (r )  with  the  value  of  found  above  and  decompose  it  into  a  linear 
part  Si  (r)  and  a  periodic  part  Sp  (r). 

(c)  Sample  s(r)  with  N  equally  spaced  points  and  find  the  periodic  spline 
that  interpolates  Sp(r),  for  ri  =  0.2  and  r2  =  0.9.  Try  various  values 
of  N  and  choose  the  one  that  (a)  is  the  smallest  possible,  (b)  gives 
a  “good”  approximation  of  the  original  ^(r),  and  (c)  yields  the  best- 
behaved  acceleration  program,  i.e.,  an  acceleration  profile  that  is  smooth 
and  within  reasonable  bounds.  Discuss  how  you  would  go  about  defining 
a  reasonable  bound. 

6.2  An  alternative  approach  to  the  solution  of  the  foregoing  smoothing  problem 
consists  in  solving  an  inverse  interpolation  problem:  Plot  the  acceleration 
program  of  the  foregoing  joint-rate  plot,  Now,  sample  a  set  of  N  equally 
spaced  points  of  s"(r)  and  store  them  in  an  A-dimensional  array  s".  Next,  find 
the  ordinates  of  the  supporting  points  of  the  interpolating  periodic  spline  and 
store  them  in  an  array  s  of  suitable  dimension.  Note  that  s"  does  not  contain 
information  on  the  linear  part  of  ^(r).  You  will  have  to  modify  suitably  your 


Fig.  6.7  A  trapezoidal  joint-rate  profile 
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array  s  so  that  it  will  produce  the  correct  abscissa  values  of  the  interpolated 
curve  s(t),  with  ^(0)  =  0  and  ^(1)  =  1.  Moreover,  s(t)  must  be  monotonic. 
Try  various  values  of  N  and  choose  the  smallest  one  that  gives  a  well-behaved 
acceleration  program,  as  described  in  Exercise  6.1. 

6.3  One  more  approach  to  smoothing  the  joint-rate  profile  of  Fig.  6.7  is  to 

use  cycloidal  motions.  To  this  end,  define  a  segment  of  a  cycloidal-motion 
function  between  r  =  0  and  r  =  ri,  so  that  ^'(ri)  =  for  as 

indicated  in  the  same  figure.  Further,  define  a  similar  segment  between  x  =  X2 
and  r  =  1  so  that  s'{x2)  =  ^  (1)  ~  Then,  join  the  two  segments 

with  a  line  of  slope  Plot  the  displacement,  velocity,  and  acceleration 
of  the  smoothed  motion.  Note  that  the  smoothed  profile  must  meet  the  end 
conditions  ^(0)  =  0  and  ^(1)  =  1,  and  that  you  may  have  to  introduce  a 
change  of  variable  to  shrink  the  corresponding  ^'(r)  segment  to  meet  these 
conditions. 

6.4  A  pick-and-place  operation  involves  picking  objects  from  a  magazine  supplied 
with  an  indexing  mechanism  that  presents  the  objects  with  a  known  pose  and 
zero  twist,  at  equal  time-intervals  T,  to  a  robot,  which  is  to  place  the  objects 
on  a  belt  conveyor  running  at  a  constant  speed  vq.  Find  5th-  and  7th-degree 
polynomials  that  can  be  suitably  used  to  produce  the  necessary  joint- variable 
time-histories. 

6.5  Repeat  Exercise  6.4,  but  now  the  objects  are  to  be  picked  up  by  the  robot  from 
a  belt  conveyor  traveling  at  a  constant  velocity  vi  and  placed  on  a  second  belt 
conveyor  traveling  at  a  constant  velocity  V2.  Moreover,  let  pi  and  p2  designate 
the  position  vectors  of  the  points  at  the  pick-  and  the  place  poses,  respectively. 
Furthermore,  the  belts  lie  in  horizontal,  parallel  planes.  Finally,  the  objects 
must  observe  the  same  attitude  with  respect  to  the  belt  orientation  in  both  the 
pick-  and  the  place  poses. 

6.6  Approximate  the  cycloidal  function  of  Sect.  6.4  using  a  periodic  cubic  spline 
with  N  subintervals  of  the  same  lengths,  for  various  values  of  N  between  5 
and  100.  Tabulate  the  approximation  error  ys.  N,  with  defined  as 

cn  =  max{e,}f 


and 


Ci  =  max  \s{r)  -  c(t)|,  t,-  <  t  <  Xi+i 

T 

in  which  ^(r)  denotes  the  spline  approximation  and  c(r)  the  cycloidal 
function.  Note:  the  cycloidal  function  can  be  decomposed  into  a  linear  and 
a  periodic  part. 

6.7  From  inspection  of  the  plot  of  the  3-4-5  polynomial  and  its  derivatives 
displayed  in  Fig.  6.2,  it  is  apparent  that  the  polynomial  can  be  regarded  as 
the  superposition  of  a  linear  and  a  periodic  function  in  the  interval  0  <  r  <  1 . 
Approximate  the  underlying  periodic  function  with  a  periodic  cubic  spline 
by  subdividing  the  above-mentioned  interval  into  N  equal  subintervals,  while 
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finding  the  value  of  N  that  will  yield  a  maximum  absolute  value  of  less  than 
10“^  in  the  error  in 

(a)  the  function  values; 

(b)  the  values  of  the  first  derivative;  and 

(c)  the  values  of  the  second  derivative. 

6.8  Repeat  Exercise  6.7  for  the  4-5-6-7  polynomial  of  Fig.  6.3. 

6.9  A  pick-and-place  operation  is  being  planned  that  should  observe  manufac¬ 
turer’s  bounds  on  the  maximum  joint  rates  delivered  by  the  motors  of  a  given 
robot.  To  this  end,  we  have  the  following  choices:  (a)  a  4-5 -6-7  polynomial; 
(b)  a  symmetric  trapezoidal  speed  profile  like  that  of  Fig.  6.7,  with  xi  =  0.20; 
and  (c)  a  cycloidal  motion.  Which  of  these  motions  produces  the  minimum 
time  in  which  the  operation  can  be  performed? 

6.10  The  maximum  speed  of  a  cycloidal  motion  was  found  to  be  2.  By  noticing  that 
the  cycloidal  motion  is  the  superposition  of  a  linear  and  a  periodic  function, 
find  a  cubic-spline  motion  that  will  yield  a  maximum  speed  of  1.5,  with  the 
characteristics  of  the  cycloidal  motion  at  its  end  points. 

6.11  The  acceleration  of  a  certain  motion  ^(r),  for  0  <  r  <  1,  is  given  at  a  sample 
of  instants  {  in  the  form 

s"{rk)  =  A^milnxk) 

Find  the  cubic  spline  interpolating  the  given  motion  so  that  its  second  time- 
derivative  will  attain  those  given  values,  while  finding  A  such  that  ^(0)  = 
0  and  ^(1)  =  1.  Hint:  A  combination  of  a  linear  function  and  a  periodic 
spline  can  yield  this  motion.  In  order  to  find  the  function  values  of  the  periodic 
spline,  exploit  the  linear  relation  between  the  function  values  and  its  second 
derivatives  at  the  spline  supporting  points,  as  discussed  in  Sect.  6.6. 

6.12  A  robotic  joint  has  been  found  to  require  to  move,  within  a  time-interval 
r,  with  a  set  of  speed  values  {0^  }f  at  equally  spaced  instants.  Find  the 
natural  cubic  spline  that  interpolates  the  underlying  motion  so  that  the  angular 
displacement  undergone  from  beginning  to  end  is  a  given  AO.  Hint:  You  will 
need  to  establish  the  linear  relation  between  the  spline  function  values  and 
those  of  its  first  derivative. 


Chapter  7 

Dynamics  of  Serial  Robotic  Manipulators 


7.1  Introduction 

The  main  objectives  of  this  chapter  are  (a)  to  devise  an  algorithm  for  the  real-time 
computed-torque  control  and  (b)  to  derive  the  system  of  second-order  ordinary 
differential  equations  (ODE)  governing  the  motion  of  an  /7-axis  manipulator.  We 
will  focus  on  serial  manipulators,  the  dynamics  of  a  much  broader  class  of  robotic 
mechanical  systems,  namely,  parallel  manipulators  and  mobile  robots,  being  the 
subject  of  Chap.  12.  Moreover,  we  will  study  mechanical  systems  with  rigid  links 
and  rigid  joints  and  will  put  aside  systems  with  flexible  elements,  which  pertain  to 
a  more  specialized  realm. 


7.2  Inverse  vs.  Forward  Dynamics 

The  two  basic  problems  associated  with  the  dynamics  of  robotic  mechanical 
systems,  namely,  the  inverse  and  the  forward  problems,  are  thoroughly  discussed 
in  this  chapter.  The  relevance  of  these  problems  cannot  be  overstated:  the  former  is 
essential  for  the  computed-torque  control  of  robotic  manipulators,  while  the  latter  is 
required  for  the  simulation  and  the  real-time  feedback  control  of  the  same  systems. 
Because  the  inverse  problem  is  purely  algebraic,  it  is  conceptually  simpler  to  grasp 
than  the  forward  problem,  and  hence,  the  inverse  problem  will  be  discussed  first. 
Moreover,  the  inverse  problem  is  also  computationally  simpler  than  the  forward 
problem.  In  the  inverse  problem,  a  time-history  of  either  the  Cartesian  or  the  joint 
coordinates  is  given,  and  from  knowledge  of  these  histories  and  the  architecture  and 
inertial  parameters  of  the  system  at  hand,  the  torque  or  force  requirements  at  the 
different  actuated  joints  are  determined  as  time-histories  as  well.  In  the  forward 
problem,  current  values  of  the  joint  coordinates  and  their  first  time-derivatives 
are  known  at  a  given  instant,  the  time-histories  of  the  applied  torques  or  forces 
being  also  known,  along  with  the  architecture  and  the  inertial  parameters  of  the 
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manipulator  at  hand.  With  these  data,  the  values  of  the  joint  coordinates  and  their 
time-derivatives  are  computed  at  a  later  sampling  instant  by  integration  of  the 
underlying  system  of  nonlinear  ordinary  differential  equations. 

The  study  of  the  dynamics  of  systems  of  multiple  rigid  bodies  is  classical,  but 
up  until  the  advent  of  the  computer,  it  was  limited  only  to  theoretical  results  and 
a  reduced  number  of  bodies.  First  Uicker  (1965)  and  then  Kahn  (1969)  produced 
a  method  based  on  the  Euler-Lagrange  equations  of  mechanical  systems  of  rigid 
bodies  that  they  used  to  simulate  the  dynamical  behavior  of  such  systems.  A  break¬ 
through  in  the  development  of  algorithms  for  dynamics  computations  was  reported 
by  Luh  et  al.  (1980),  who  proposed  a  recursive  formulation  of  multibody  dynamics 
that  is  applicable  to  systems  with  serial  kinematic  chains.  This  formulation,  based 
on  the  Newton-Euler  equations  of  rigid  bodies,  allowed  the  calculation  of  the 
joint  torques  of  a  six-revolute  manipulator  with  only  800  multiplications  and  595 
additions,  a  tremendous  gain  if  we  consider  that  the  straightforward  calculation  of 
the  Euler-Lagrange  equations  for  the  same  type  of  manipulator  involves  66,271 
multiplications  and  51,548  additions,  as  pointed  out  by  Hollerbach  (1980).  In 
the  foregoing  reference,  a  recursive  derivation  of  the  Euler-Lagrange  equations 
was  proposed,  whereby  the  computational  complexity  was  reduced  to  only  2,195 
multiplications  and  1,719  additions. 

The  foregoing  results  provoked  a  discussion  on  the  merits  and  demerits  of 
each  of  the  Euler-Lagrange  and  the  Newton-Euler  formulations.  Silver  (1982) 
pointed  out  that  since  both  formulations  are  equivalent,  they  should  lead  to  the 
same  computational  complexity.  In  fact.  Silver  showed  how  to  derive  the  Euler- 
Lagrange  equations  from  the  Newton-Euler  formulation  by  following  an  approach 
first  introduced  by  Kane  (1961)  in  connection  with  nonholonomic  systems.  Kane 
and  Levinson  (1983)  then  showed  how  Kane’s  equations  can  be  applied  to  particular 
robotic  manipulators  and  arrived  at  lower  computational  complexities.  They  applied 
the  said  equations  to  the  Stanford  Arm  (Paul  1981)  and  computed  its  inverse 
dynamics  with  646  multiplications  and  394  additions.  Thereafter,  Khalil  et  al. 
(1986)  proposed  a  condensed  recursive  Newton-Euler  method  that  reduced  the 
computational  complexity  to  538  multiplications  and  478  additions,  for  arbitrary 
architectures.  Further  developments  in  this  area  were  reported  by  Balafoutis  and 
Patel  (1991),  who  showed  that  the  underlying  computational  complexity  can  be 
reduced  to  489  multiplications  and  420  additions  for  the  most  general  case  of  a  six- 
revolute  manipulator,  i.e.,  without  exploiting  particular  features  of  the  manipulator 
geometry.  Balafoutis  and  Patel  based  their  algorithm  on  tensor  analysis,  whereby 
tensor  identities  are  exploited  to  their  fullest  extent  in  order  to  reduce  the  number 
of  operations  involved.  Li  and  Sankar  (1992),  in  turn,  reported  further  savings  that 
allowed  them  to  bring  down  those  numbers  to  459  multiplications  and  390  additions. 

In  this  chapter,  the  inverse  dynamics  problem  is  solved  with  the  well-known 
recursive  Newton-Euler  algorithm,  while  the  forward  dynamics  problem  is  handled 
with  a  novel  approach,  based  on  the  reciprocity  relations  between  the  constraint 
wrenches  and  the  feasible  twists  of  a  manipulator.  This  technique  is  developed  with 
the  aid  of  a  modeling  tool  known  as  the  natural  orthogonal  complement,  thoroughly 
discussed  in  Sect.  7.5. 
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Throughout  the  chapter,  we  will  follow  a  multibody  system  approach,  which 
requires  a  review  of  the  underlying  fundamentals. 


7.3  Fundamentals  of  Multibody  System  Dynamics 


7.3.1  On  Nomenclature  and  Basic  Definitions 


We  consider  here  a  mechanical  system  composed  of  r  rigid  bodies  and  denote  by 
M;  the  6x6  inertia  dyad — see  Sect.  3.8 — of  the  ith  body.  Moreover,  we  let  W/, 
already  introduced  in  Eq.  (3.140),  be  the  6x6  angular-velocity  dyad  of  the  same 
body.  As  pertaining  to  the  case  at  hand,  the  said  matrices  are  displayed  below: 


I.  O 

O  niil 


(7.1) 


where  1  and  O  denote  the  3x3  identity  and  zero  matrices,  respectively,  while 
and  1/  are  the  angular- velocity  and  the  inertia  matrices  of  the  ith  body,  this  last 
being  defined  with  respect  to  the  center  of  mass  Q  of  this  body.  Moreover,  the 
mass  of  this  body  is  denoted  by  ,  whereas  C/  and  C/  denote  the  position  and  the 
velocity  vectors  of  Q  in  an  inertial  frame.  Furthermore,  let  t/  denote  the  twist  of 
the  same  body,  the  latter  being  defined  in  terms  of  the  angular  velocity  vector  o)/, 
the  vector  of  and  the  velocity  of  Q.  The  six-dimensional  momentum  screw  jii 
is  defined  likewise.  Furthermore,  and  wf  are  defined  as  the  working  wrench 
and  the  nonworking  constraint  wrench  exerted  on  the  ith  body  by  its  neighbors,  in 
which  forces  are  assumed  to  be  applied  at  Q .  We  thus  have,  for  /  =  1 , . . . ,  r , 


ti  = 
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Ci 
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.if, 


(7.2) 


where  superscripted  n,  and  f,  stand,  respectively,  for  the  moment  and  the  force 
acting  on  the  ith  body,  the  force  being  applied  at  the  mass  center  Q .  Thus,  whereas 
vfY  accounts  for  forces  and  moments  exerted  by  both  the  environment  and  the 
actuators,  including  driving  forces  as  well  as  dissipative  effects,  wp,  whose  sole 
function  is  to  keep  the  links  together,  accounts  for  those  forces  and  moments  exerted 
by  the  neighboring  links,  which  do  not  produce  any  mechanical  work.  Therefore, 
friction  wrenches  applied  by  the  (/  —  l)st  and  the  (/  +  l)st  links  onto  the  ith  link 
are  not  included  in  wp ;  rather,  they  are  included  in  . 

Clearly,  from  the  definitions  of  ,  and  it ,  we  have 


II  i  =  MAz 


(7.3) 
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Moreover,  from  Eq.  (3.143), 


=  M,-t/  +  =  Miti  + 


(7.4) 


We  now  recall  the  Newton-Euler  equations  for  a  rigid  body,  namely. 


(7.5a) 

(7.5b) 


which  can  be  written  in  compact  form  using  the  foregoing  six-dimensional  twist  and 
wrench  arrays  as  well  as  the  6x6  inertia  and  angular- velocity  dyads.  We  thus  obtain 
the  Newton-Euler  equations  of  the  i  th  body  in  the  form 


M,-t  =  +  wf'  +  wf 


(7.5c) 


7.3.2  The  Euler-Lagrange  Equations  of  Serial  Manipulators 

The  Euler-Lagrange  dynamical  equations  of  a  mechanical  system  are  now  recalled, 
as  pertaining  to  serial  manipulators.  Thus,  the  mechanical  system  at  hand  has  n 
degrees  of  freedom,  its  n  independent  generalized  coordinates  being  the  n  joint 
variables,  which  are  stored  in  the  w -dimensional  vector  0 .  We  thus  have 


(7.6) 


where  T  is  a  scalar  function  denoting  the  kinetic  energy  of  the  system  and  0  is  the 


-dimensional  vector  of  generalized  force.  If  some  forces  on  the  right-hand  side 
stem  from  a  potential  E,  we  can,  then  decompose  0  into  two  parts,  0^  and  0^,  the 
former  arising  from  V  and  termed  the  conservative  force  of  the  system;  the  latter  is 
the  nonconservative  force.  That  is. 


(7.7) 


the  above  Euler-Lagrange  equations  thus  becoming 


(7.8) 


where  L  is  the  Lagrangian  of  the  system,  defined  as 


L  =  T  -V 


(7.9) 


^See  Exercise  7.1  for  an  extension  of  this  relation  to  a  system  of  n  rigid  bodies. 


7.3  Fundamentals  of  Multibody  System  Dynamics 


285 


Moreover,  the  kinetic  energy  of  the  system  is  simply  the  sum  of  the  kinetic  energies 
of  all  the  r  links.  Recalling  Eq.  (3.145),  which  gives  the  kinetic  energy  of  a  rigid 
body  in  terms  of  six-dimensional  arrays,  one  has 


T  =  J2Ti 

1 


(7.10) 


whereas  the  vector  of  nonconservative  generalized  forces  is  given  by 


0 


do 


9  A 

do 


(7.11) 


in  which  Ft^  and  A  denote  the  power  supplied  to  the  system  and  the  Rayleigh 
dissipation  function,  or  for  brevity,  the  dissipation  function  of  the  system. 

The  first  of  these  items  is  discussed  below;  the  latter  is  only  outlined  in  this 
section  but  is  discussed  extensively  in  Sect.  7.8.  First,  the  wrench  vff  is  decomposed 
into  two  parts,  and  wf ,  the  former  being  the  wrench  supplied  by  the  actuators 
and  the  latter  being  the  wrench  that  arises  from  viscous  and  Coulomb  friction,  the 
gravity  wrench  being  not  needed  here  because  gravity  effects  are  considered  in  the 
potential  V{0).  We  thus  call  the  active  wrench  and  wf  the  dissipative  wrench. 
Here,  the  wrenches  supplied  by  the  actuators  are  assumed  to  be  prescribed  functions 
of  time.  Moreover,  these  wrenches  are  supplied  by  single-dof  actuators  in  the  form 
of  forces  along  a  line  of  action  or  moments  in  a  given  direction,  both  line  and 
direction  being  fixed  to  the  two  bodies  that  are  coupled  by  an  active  joint.  Hence, 
the  actuator-supplied  wrenches  are  dependent  on  the  posture  of  the  manipulator 
as  well,  but  not  on  its  twist.  That  is,  the  actuator  wrenches  are  functions  of  both 
the  vector  of  generalized  coordinates,  or  joint  variables,  and  time,  but  not  of  the 
generalized  speeds,  or  joint-rates.  Forces  dependent  on  the  latter  to  be  considered 
here  are  assumed  to  be  all  dissipative.  As  a  consequence,  they  can  be  readily 
incorporated  into  the  mathematical  model  at  hand  via  the  dissipation  function,  to 
be  discussed  in  Sect.  7.8.  Note  that  feedback  control  schemes  require  actuator  forces 
that  are  functions  not  only  of  the  generalized  coordinates,  but  also  of  the  generalized 
speeds.  These  forces  or  moments  are  most  easily  incorporated  into  the  underlying 
mathematical  model,  once  this  model  is  derived  in  the  state-variable  space,  i.e.,  in 
the  space  of  generalized  coordinates  and  generalized  speeds. 

Thus,  the  power  supplied  to  the  ith  link,  Ilf,  is  readily  computed  as 

uf  =  (wffti  (7.12a) 


Similar  to  the  kinetic  energy,  then,  the  power  supplied  to  the  overall  system  is 
simply  the  sum  of  the  individual  powers  supplied  to  each  link,  and  expressed  as  in 
Eq.  (7.12a),  i.e.. 


1 


(7.12b) 
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Further  definitions  are  now  introduced.  These  are  the  6/7 -dimensional  vectors  of 
manipulator  twist,  t;  manipulator  momentum,  fi;  manipulator  constraint  wrench, 
w^;  manipulator  active  wrench,  and  manipulator  dissipative  wrench,  w^. 
Additionally,  the  6n  x  6n  matrices  of  manipulator  mass,  M,  and  manipulator 
angular  velocity,  W,  are  also  introduced  below: 


1 

1 _ 

1 

1 _ 

1 

•• 

1 _ 

,  w  = 

w"* 

(7.13a) 


(7.13b) 


M  =  diag(Mi,  ...,M„),  W  =  diag(Wi,  . . . ,  W,  )  (7.13c) 


It  is  now  apparent  that,  from  definitions  (7.13b  and  7.13c)  and  relation  (7.3),  we 
have 


p.  =  Mt  (7.14) 

Moreover,  from  definitions  (7.1)  and  (7.2), 

^=Mt  +  WMt  (7.15) 

With  the  foregoing  definitions,  then,  the  kinetic  energy  of  the  manipulator  takes  a 
simple  form,  namely. 


_  1  'T’  1 

T  =  -t^Mt  =  p 
2  2 


(7.16) 


which  is  a  quadratic  form  in  the  system  twist.  Since  the  twist,  on  the  other  hand,  is 
a  linear  function  of  the  vector  0  of  joint  rates,  the  kinetic  energy  turns  out  to  be  a 
quadratic  form  in  the  vector  of  joint  rates.  Moreover,  we  will  assume  that  this  form 
is  homogeneous  in  0 ,  i.e.. 


1  T 

T  =  -e  1(0)0  (7.17) 

Notice  that  the  above  assumption  implies  that  the  base  of  the  robot  is  fixed  to  an 
inertial  base,  and  hence,  when  all  joints  are  locked,  the  kinetic  energy  of  the  robot 
vanishes,  which  would  not  be  the  case  if,  for  example,  the  robot  were  mounted  on 
the  International  Space  Station.  If  this  were  the  case,  then  the  kinetic  energy  would 
not  vanish  even  if  all  robot  joints  were  locked,  which  means  that  the  foregoing 
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kinetic-energy  expression  would  include  a  linear  term  in  0  and  a  term  independent 
of  the  joint-rates.  In  any  event,  it  is  apparent  that 


(7.18) 


which  means  that  the  n  n  generalized  inertia  matrix  is  the  Hessian  matrix  of  the 
kinetic  energy  with  respect  to  the  vector  of  generalized  speed. 

Furthermore,  the  Euler-Lagrange  equations  can  be  written  in  the  form 


d  fdT\  dT  dv 

77  VTJ  j  ~  97  TT" 


(7.19a) 


Now,  from  the  form  of  T  given  in  Eq.  (7.17),  the  partial  derivatives  appearing  in  the 
foregoing  equation  take  the  forms  derived  below: 


=  \{e)0 


and  hence, 


d  fdT\  . 

Moreover,  in  order  to  calculate  the  second  term  of  the  left-hand  side  of  Eq.  (7.19a), 
we  express  the  kinetic  energy  in  the  form 

T=^-i^{ejYe  (7.19c) 


where  p(^ ,  ^)  is  the 


generalized  momentum  of  the  manipulator,  defined  as 


Hence, 


or 


p(^,^)^I(^)^ 


(7.19d) 


d0  2  \  d0  ) 


(7.19e) 


1  9(1(9) 

2  961 


(7.19f) 


the  Euler-Lagrange  equations  thus  taking  on  the  alternative  form 
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Fig.  7.1  A  planar 
manipulator 


P(x,  y) 


i(^)^  +i{ej)e 


T 


1  3(119) 

2  3(9 


e  + 


dv 

Jo 


(7.20) 


Example  7.3.1  (Euler-Lagrange  Equations  of  a  Planar  Robot).  Consider  the 
manipulator  of  Fig.  7.1,  with  links  designed  so  that  their  centers  of  mass,  Ci, 
C2,  and  C3,  are  located  at  the  midpoints  of  segments  O1O2,  O2O3,  and  O3P, 
respectively.  Moreover,  the  ith  link  has  a  mass  and  a  centroidal  moment  of 
inertia  in  a  direction  normal  to  the  plane  of  motion  f ,  while  the  joints  are  actuated 
by  motors  delivering  torques  ri,  X2,  and  13,  the  lubricant  of  the  joints  producing 
dissipative  torques  that  we  will  neglect  in  this  model.  Under  the  assumption  that 
gravity  acts  in  the  direction  of  —  F,  find  the  associated  Euler-Lagrange  equations. 

Solution:  Here  we  recall  the  kinematic  analysis  of  Sect.  5.7  and  the  definitions 
introduced  therein  for  the  analysis  of  planar  motion.  In  this  light,  all  vectors 
introduced  below  are  two-dimensional,  the  scalar  angular  velocities  of  the  links, 
coi,  for  /  =  1,  2,  3,  being 


o)i  —  Oi,  0)2  —  J  0)2  —  J  J  ^3 


Moreover,  the  velocities  of  the  centers  of  mass  are 
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Cl  = 


C2  = 


C3  = 


1  . 

-^lEai 

2 

^lEai  +  2^^!  02)Ea2 


1  . 


^lEai  +  (^1  +  02)Ea2  +  2^^^  +  ^2  H“  ^3)Ea3 


the  kinetic  energy  then  becoming 


1  3 

T  =  -  ^{ntiWcif  +  haf) 


The  squared  magnitudes  of  the  mass-center  velocities  are  now  computed  using  the 
expressions  derived  above.  After  simplifications,  these  yield 


Cl 


C2 


C3 


1 


2^2 


-aiOi 
4  ^  ^ 


1 


20162  +  ^2)  1^22  cos  02(^f  H”  ^1^2) 


+  a^iOi  +  201^2  +  ^2) 

O2  0^  201^2  H”  201^3  +  202^3) 

+  2^21^22  COS  ^2(^f  H”  ^1^2)  H”  t2it23  COS(02  H”  ^3)(^f  +  01^2  H”  ^1^3) 
+^22^23  COS  62  26162  +  01^3-1-  6262) 


.2/^2 


n2^ 


The  kinetic  energy  of  the  whole  manipulator  thus  becomes 


1 


T  —  -(/ii^^  +  2I126162  +  2I236263  +  I2262  +  2I136163  +  I3363) 


n2 


n2^ 


with  coefficients  lij ,  for  /  =  1,  2,  3,  and  j  =  /  to  3  being  the  distinct  entries  of  the 
3x3  matrix  of  generalized  inertia  of  the  system.  These  entries  are  given  below: 


/ii  ^ 


h  h  h  -^rn\a\  +  m2  +  -<22  +  a\a2C2 
+m3  (a\  +  <22  +  -<23  +  2a\a2C2  +  <2i<23C23  +  a2a3C3 


1 

/12  =  /2  +  ^3  +  “ 


1  2 

m2  I  -<22  +  <2i<22C2 
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h3 
hi 
hi 
hi 

where  Ci  and  Ctj  stand  for  cos  Ot  and  cos(0,  +  ^7  )’  respectively.  From  the  foregoing 
expressions,  it  is  apparent  that  the  generalized  inertia  matrix  is  not  a  function  of  Oi, 
which  is  only  natural,  for  if  the  second  and  third  joints  are  locked  while  leaving  the 
first  one  free,  the  whole  manipulator  becomes  a  single  rigid  body  pivoting  about 
point  Oi .  Now,  the  polar  moment  of  inertia  of  a  rigid  body  in  planar  motion  about 
a  fixed  point  is  constant,  and  hence,  the  first  joint  variable  should  not  affect  the 
generalized  inertia  matrix. 

Furthermore,  the  potential  energy  of  the  manipulator  is  computed  as  the  sum  of 
the  individual  link  potential  energies,  i.e.. 


+  m3  (  2a\  +  -^3  +  2aia2C2  +  aia2C2i  +  2a2a2C2 


1 


1 


h  +  ^1^3C23  +  a2«3C3  j 

I2  +  h  +  ^^3  f'^2  +  +  ‘^2<23C3 


h  +  i"J3  [hi  +  a2a3C3 


1  2 

h  +  -m3a^ 


V  =  \-miga\s,m9\  +  m2g 


+m3g 


a\  sin  H — a2  sin(0i  +  ^2) 

^2j 


1 


sin «,+<„sin(e, +%)  +  -<,,  since, +«,  +  «,) 


while  the  total  power  delivered  to  the  manipulator  takes  the  form 


n  —  Ti^i  +  T202  H”  "^3^3 


We  now  proceed  to  compute  the  various  terms  in  Eq.  (7.20).  We  already  have 
i(^),  but  we  do  not  have,  as  yet,  its  time-derivative.  However,  the  entries  of  I  are 
merely  the  time-derivatives  of  the  entries  of  I.  From  the  above  expressions  for  these 
entries,  their  time-rates  of  change  are  readily  calculated,  namely. 


•  •  •  •  •  • 

hi  =  -m2aia2S2d2  -  m3[2aia2S2d2  +  aia3S23{02  +  O3)  +  02033363] 

in  =  +»,«,«>(%  +  ft)  +  2ns«,ssft|) 

•  •  •  • 

■^13  =  -2:m3[a\a3S23{02  +  63)  +  02033363] 
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•  • 

I22  =  -m^a2aiS^9i 

h,  = 

hs  =  0 

with  Sij  defined  as  sm(0i  +  Oj).  It  should  now  be  apparent  that  the  time-rate  of 
change  of  the  generalized  inertia  matrix  is  independent  of  0i ,  as  one  should  have 
expected,  for  this  matrix  is  independent  of  Oi.  That  is,  if  all  joints  but  the  first  one 
are  frozen,  no  matter  how  fast  the  first  joint  rotates,  the  manipulator  moves  as  a 
single  rigid  body  whose  polar  moment  of  inertia  about  Oi ,  the  center  of  the  first 
joint,  is  constant.  As  a  matter  of  fact,  1 33  is  constant  for  the  same  reason  and  1 33 
hence  vanishes.  We  have,  then,^ 


10  =L  = 


hlOi  +  I12O2  +  /l3^3 
iuOl  +  122^2  +  ^23^3 
iuOi  -h  /23^2 


whose  components,  if,  for  /  =  1,  2,  3,  are  readily  calculated  as 


-[m2aia2S2  +  m3ai(2a2S2  +  <33^23)]^! ^2  -  ni3a3{aiS23  +  <22^3)^! ^3 

1 
2 


--[m2aia2S2  +  m3ai(2a2S2  +  <3!3^23)]^|  -m3a3{aiS23  +  <3!2^3)4^3 


1 


^2  — 


^3  — 


--m3a3{aiS23  +  a2S3)0^ 

1  .  . 

--[m2aia2S2  +  m3ai(2a2S2  +  <3!3^23)]^i^2 

-1,»3«,(<.,3b  +«33,)9,9,  -».3«3<.,»,93<j3  - 


1 


32 


The  next  term  in  the  right-hand  side  of  Eq.  (7.20)  now  requires  the  calculation 
of  the  partial  derivatives  of  vector  10  with  respect  to  the  joint  variables,  which  are 
computed  below.  Let 


9(1^) 

de 


is  the  Greek  letter  iota  and  denotes  a  vector;  according  to  our  notation,  its  components  are  o,  ^2, 
and  63. 
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its  entries  being  denoted  by  I'-j .  This  matrix,  in  component  form,  is  given  by 


t  = 


0  +  hi, 2^2  +  hi, 2^2  hi, 2^1  +  h2,2^2  +  ^3,3^3 

0  hl,!^!  +  +  ^23,2^3  ^2,3^1  +  ^22,3^2  +  ^23,3^3 

LO  /l3,2^1  +  h2,2^2  +  h2,2^2  ^13,3^1  +  ^23,3^2  +  ^33,3^3. 


with  the  shorthand  notation  hj^k  indicating  the  partial  derivative  of  hj  with  respect 
to  Ok .  As  the  reader  can  verify,  these  entries  are  given  as 


=  0 


11 

I'n  =  -[m2axa2S2  +  m2{2aia2S2  +  aia3-523)]^i 

1  .  1  . 

--[m2aia2S2  +  m2(2aia2S2  +  aia3-523)]^2  -  -W3aia3^2303 

+  a,a,s,)ei,  -  i™, 

1 


13 


In 


J' 

^22 


J' 

^22 


Hi 


J' 

^22 


J' 

^22 


=  0 


1 


--\,n,a,a,s,  +  ,„,aa,a,s,  +  a,a,sn)V, 

1  .  .  1  . 

--m^{aia^S22  +  2a2a2Sh0i  -  m3(22<23^302  -  -m3(22<23^303 


=  0 


1  . 

-m3aia3S220i 

1 

-m3(aia3S23  +  <22(23^3) 


1  . 

-m3(22<23^36>2 


Now,  we  define  the  three-dimensional  vector  y  below: 


y  = 


dm 

do 


-iT 


0 


its  three  components, )/, ,  for  i  =  1,  2,  3,  being 
Yi  =0 

Yi  =  -[m2aia2S2  +  m2(2aia2S2  +  aia2S22)]0l 

•  • 

-[m2aia2^2  +  m2(2aia2S2  +  aia2S23)]0i02 
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•  • 

-m3aia3S239i93 

ys  =  -m3(aia3S23  +  02^3-53)^?  -  m3(aia3S23  +  202^3-53)014 
-m3{a\a3S23  +  a2«3-53)44  -  W302«3-534  “  W302«3-5344 


We  now  turn  to  the  computation  of  the  partial  derivatives  of  the  potential  energy: 


dV 

Wi 

dV 

W2 

dV 


1 


1 


1 


=  -migaiCi  +  m2g  ^oici  +  -02C12  j  +  m3g  ^oici  +  a2Cn  +  -«3Ci23 

=  +  m,s 

1 

=  -m3ga3Cu3 


The  Euler-Lagrange  equations  thus  reduce  to 


••  ••  ••  ft 

hiOi  +  I12O2  +  /i3^3  +  D  —  -yi  + 

+m2g(oiCi  +  ^02^12)  +  m3g(aici  +  02^12  +  ^^3^123)  = 

•  •  ••  ••  11 

/l201  +  I22O2  +  ^23^3  +  ^2  —  -yi  +  -^2g(^2Cl2 

+  n„Ha,cn  +  y,cn,)  =  Tj 

•  •  ••  ••  11 

^13^1  +  ^23^2  +  ^33^3  +  ^3  “  “73  +  “^3^^3<^123  =  "^3 


With  this  example,  it  becomes  apparent  that  a  straightforward  differentiation 
procedure  to  derive  the  Euler-Lagrange  equations  of  a  robotic  manipulator,  or 
for  that  matter,  of  a  mechanical  system  at  large,  is  not  practical.  Eor  example, 
these  equations  do  not  seem  to  lend  themselves  to  symbolic  manipulations  for 
a  six-axis  manipulator  of  arbitrary  architecture,  given  that  they  become  quite 
cumbersome  even  for  a  three-axis  planar  manipulator  with  an  architecture  that  is  not 
so  general.  Eor  this  reason,  procedures  have  been  devised  that  lend  themselves  to  an 
algorithmic  treatment.  We  will  study  a  procedure  based  on  the  natural  orthogonal 
complement  whereby  the  underlying  equations  are  derived  using  matrix-times- 
vector  multiplications. 


73,3  Kane^s  Equations 

Kane ’s  equations  (Kane  and  Levinson  1983),  sometimes  referred  to  as  D  ’Alembert’s 
equations  in  Lagrangianform  are  also  useful  in  robot  dynamics  (Angeles  et  al.  1989). 
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A  feature  of  Kane’s  equations  is  that  they  are  derived  from  the  free-body  diagrams 
of  the  various  rigid  bodies  constituting  the  multibody  system  at  hand.  Upon 
introducing  generalized  coordinates  a  la  Lagrange,  the  mathematical  model  of  the 
system  is  derived,  which  is  equivalent  to  the  underlying  Euler-Lagrange  equations. 
Kane’s  equations  take  a  rather  simple  form,  for  an  n-&oi  mechanical  system,  namely, 

0  +  0*  =0 


where  0  and  0*  are  the  /7 -dimensional  vectors  of  generalized  active  force  and  inertia 
force,  respectively.  With  the  notation  introduced  above,  these  vectors  are  given  by 


(7.21a) 


and 


0 


* 


T 

(I/O),-  +  (Oi  X  li(Oi) 


(7.21b) 


In  the  above  expressions,  q  =  d{{/dt  the  -dimensional  vector  of  generalized 
speeds  in  Kane’s  terminology,  while  the  ^  x  3  matrices  96/  / 9q  and  9a)/  / 9q  are  the 
partial  rates  of  change  of  mass-center  velocity  and  angular  velocity  of  the  ith  rigid 
body. 


7.4  Recursive  Inverse  Dynamics 

The  inverse  dynamics  problem  associated  with  serial  manipulators  is  studied  here. 
We  assume  at  the  outset  that  the  manipulator  under  study  is  of  the  serial  type  with 
n  1  links  including  the  base  link  and  n  joints  of  either  the  revolute  or  the  prismatic 
type. 

The  underlying  algorithm  consists  of  two  steps:  (a)  kinematic  computations, 

required  to  determine  the  twists  of  all  the  links  and  their  time  derivatives  in  terms 

•  •  • 

of  0,  0,  and  0  ;  and  (b)  dynamic  computations,  required  to  determine  both  the 
constraint  and  the  external  wrenches.  Each  of  these  steps  is  described  below,  the 
aim  here  being  to  calculate  the  desired  variables  with  as  few  computations  as 
possible,  for  one  purpose  of  inverse  dynamics  is  to  permit  the  real-time  model- 
based  control  of  the  manipulator.  Real-time  performance  requires,  obviously,  a  low 
number  of  computations.  Eor  the  sake  of  simplicity,  we  decided  against  discussing 
the  algorithms  with  the  lowest  computational  cost,  mainly  because  these  algorithms, 
fully  discussed  by  Balafoutis  and  Patel  (1991),  rely  heavily  on  tensor  calculus, 
which  we  have  not  studied  here.  With  the  notation  introduced  in  Chap.  4,  revolute 
joints  are  referred  to  as  R,  prismatic  joints  as  P. 
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7.4.1  Kinematics  Computations:  Outward  Recursions 

We  will  use  the  Denavit-Hartenberg  (DH)  notation  introduced  in  Sect.  4.2  and  hence 
will  refer  to  Fig.  4.9  for  the  basic  notation  required  for  the  kinematic  analysis  to  be 
described  first.  Note  that  the  calculation  of  each  Q,  matrix,  as  given  by  Eq.  (4.1e), 
requires  four  multiplications  and  zero  additions. 

Moreover,  every  three-dimensional  vector- component  transfer  from  the  Ti  frame 
to  the  frame  requires  a  multiplication  by  Qf .  Likewise,  every  component 
transfer  from  the  J^i+\  frame  to  the  Ti  frame  requires  a  multiplication  by  Q,. 
Therefore,  we  will  need  to  account  for  the  aforementioned  component  transfers, 
which  we  will  generically  term  coordinate  transformations  between  successive 
coordinate  frames.  We  derive  below  the  number  of  operations  required  for  such 
transformations.  If  we  have  [r]i  =  [ri,  r2,  r^]^  and  we  need  [r]/+i,  then  we 
proceed  as  follows: 


[r],+i  =Qnr],-  (7.22) 

and  if  we  recall  the  form  of  Q,  from  Eq.  (4.1e),  we  then  have 


COS  Of 

sin  Of 

O' 

~ri~ 

ri  cos  0i  +  r2  sin  0,- 

[rL+i  = 

—Xi  sin  Oi 

Xi  cos  Oi 

l^i 

ri 

— 

-Xi  r  +  lii  r3 

(7.23a) 

_  fii  sin  Oi 

-p.i  cos  Oi 

J3_ 

liir+Xjrs 

where  A/  =  cos  o';  and  /X/  =  sin  o';  ,  while 

r  =  ri  sin  Of  —  r2  cos  Of  (7.23b) 

Likewise,  if  we  have  [v]/+i  =  [vi,  V2,  r’3]^  and  we  need  [v]/,  we  use  the 
component  transformation  given  below: 


cos  Oi 

—Xi  sin  Oi 

IJLi  sin  di 

~Vl~ 

vi  COS  Oi  —  V  sin  Oi 

[v],-  = 

sin  Oi 

Xi  cos  Oi 

-jii  cos  6i 

V2 

— 

vi  sin  Oi  +  V  cos  Oi 

(7.24a) 

0 

l^i 

h  _ 

_^3_ 

_  V2P^i  +  V^Xi  _ 

where 


V  =  02^1  -  v^iii  (7.24b) 

It  is  now  apparent  that  every  coordinate  transformation  between  successive 
frames,  whether  forward  or  backward,  requires  eight  multiplications  and  four 
additions.  Here,  as  in  Chap.  5,  we  indicate  the  units  of  multiplications  and  additions 
with  M  and  A,  respectively. 
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Fig.  7.2  A  revolute  joint 


The  angular  velocity  and  acceleration  of  the  i  th  link  are  computed  recursively  as 
follows: 


0)i 


(Of -I  +  Oiti ,  if  the  /  th  joint  is  R 
(Oi-i,  if  the  i  th  joint  is  P 


(7.25a) 


•  •  • 

(bi-i  (Of -I  X  Oiti  +  Oiti,  if  the  /  th  joint  is  R 
(Oi-i,  if  the  /  th  joint  is  P 


(7.25b) 


for  /  =  1,  2,  . . . ,  where  coq  and  coq  are  the  angular  velocity  and  angular 
acceleration  of  the  base  link.  Note  that  Eqs.  (7.25a  and  b)  are  frame-invariant; 
i.e.,  they  are  valid  in  any  coordinate  frame,  as  long  as  the  same  frame  is  used  to 
represent  all  quantities  involved.  Below  we  derive  the  equivalent  relations  applicable 
when  taking  into  account  that  quantities  with  a  subscript  /  are  available  in  J-’i+i- 
coordinates.  Hence,  operations  involving  quantities  with  different  subscripts  require 
a  change  of  coordinates,  which  is  taken  care  of  by  the  corresponding  rotation 
matrices. 

In  order  to  reduce  the  numerical  complexity  of  the  algorithm  developed  here, 
all  vector  and  matrix  quantities  of  the  /th  link  will  be  expressed  in  Note, 

however,  that  the  two  vectors  e,  and  +i  are  fixed  to  the  /  th  link,  which  is  a  potential 
source  of  confusion.  Now,  since  e,  has  an  extremely  simple  form  in  ,  namely, 
[0,  0,  1  ]^,  this  will  be  regarded  as  a  vector  of  the  (/  —  l)st  link,  which  is  fixed 
to  P'i — see,  e.g..  Fig.  7.2.  Therefore,  this  vector,  or  multiples  of  it,  will  be  added 
to  vectors  bearing  the  (/  —  l)st  subscript  without  any  coordinate  transformation. 
Moreover,  subscripted  brackets,  as  introduced  in  Sect.  2.2,  can  be  avoided  if  all 
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vector  and  matrix  quantities  subscripted  with  i ,  except  for  vector  ,  are  assumed 
to  be  expressed  in  Furthermore,  in  view  of  the  serial  type  of  the  underlying 
kinematic  chain,  only  additions  of  quantities  with  two  successive  subscripts  will 
appear  in  the  relations  below. 

Quantities  given  in  two  successive  frames  can  be  added  if  both  are  expressed 
in  the  same  frame,  the  obvious  frame  of  choice  being  the  frame  of  one  of  the  two 
quantities.  Hence,  all  we  need  to  add  two  quantities  with  successive  subscripts  is 
to  multiply  one  of  these  by  a  suitable  orthogonal  matrix.  Additionally,  in  view  of 
the  outwards  recursive  nature  of  the  foregoing  kinematic  relations,  it  is  apparent 
that  a  transfer  from  -  to  /'i+i -coordinates  is  needed,  which  can  be  accomplished 
by  multiplying  either  or  any  other  vector  with  the  {i  —  1)  subscript  by  matrix 
QJ  .  Hence,  the  angular  velocities  and  accelerations  are  computed  recursively,  as 
indicated  below: 


(^i 


Qf  (coi-i  +  diet),  if  the  /th  joint  is  R 
Qf  (Oi-i,  if  the  i  th  joint  is  P 

•  •  • 

Qf  -h  (Oi-i  X  if  the  /  th  joint  is  R 

Qf  6)i-i ,  if  the  i  th  joint  is  P 


(7.26a) 


(7.26b) 


If  the  base  link  is  an  inertial  frame,  then 


COq  —  0,  COq  —  0 


(7.27) 


Thus,  calculating  each  coj  vector  in  P'i+i  when  coj-i  is  given  in  P’f  requires  8M 

and  5 A  if  the  /th  joint  is  R;  if  it  is  P,  the  said  calculation  reduces  to  8M  and  4 A. 

*  * 

Here,  note  that  Oiti  =  [0,  0,  Oi  Y  in  /^  -coordinates,  and  hence,  the  vector  addition 
of  the  upper  right-hand  side  of  Eq.  (7.26a)  requires  only  \A.  Furthermore,  in  order 
to  determine  the  number  of  operations  required  to  calculate  coi  in  when  oot-i 
is  available  in  Ti ,  we  note  that 


Moreover,  we  let 


0)x 

COy 

.^Z 


(128) 


(7.29) 


Hence, 
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Fig.  7.3  A  prismatic  joint 


X  ], 


Oi  COy 

-Oi 


Furthermore,  we  note  that 


[Oiei]i 


0 

0 


(7.30) 


(7.31) 


and  hence,  the  calculation  of  (Oi  in  /’z+i  when  (Oi-i  is  given  in  Ti  requires  lOM 
and  7^  if  the  /th  joint  is  R;  if  it  is  P,  the  same  calculation  requires  8M  and  4 A. 

Furthermore,  let  C/  be  the  position  vector  of  Q ,  the  center  of  mass  of  the  i  th 
link.  Pi  being  the  vector  directed  from  Oi  to  Q,  as  shown  in  Figs.  7.2  and  7.3.  The 
position  vectors  of  two  successive  centers  of  mass  thus  observe  the  relationships 

(a)  if  the  i  th  joint  is  R,  then  from  Fig.  7.2, 


8i-i  =  SLi-i  -  pi_^  (7.32a) 

Cf  =  Cf-i  +  +  Pi  (7.32b) 

(b)  if  the  i  th  joint  is  P,  then  from  Fig.  7.3, 

8i-i  =  d,_i  -  pi_^  (7.32c) 

Ci  =  Cf-i  +  +  bief  +  Pi  (7.32d) 
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where  point  Ot,  in  this  case,  is  a  point  of  the  (/  —  l)st  link  conveniently  defined, 
as  dictated  by  the  particular  geometry  of  the  manipulator  at  hand.  The  foregoing 
freedom  in  the  choice  of  Ot  is  a  consequence  of  prismatic  pairs  having  only  a 
defined  direction  but  no  axis,  properly  speaking. 

Notice  that  in  the  presence  of  a  revolute  pair  at  the  /th  joint,  the  difference 
a;_i  —  is  constant  in  Ti.  Likewise,  in  the  presence  of  a  prismatic  pair  at  the 
same  joint,  the  difference  d;  _i  —  is  constant  in  Ti .  Therefore,  these  differences 
are  computed  off-line,  their  evaluation  not  counting  toward  the  computational 
complexity  of  the  algorithm. 

Upon  differentiation  of  both  sides  of  Eqs.  (7.32b  and  d)  with  respect  to  time,  we 
derive  the  corresponding  relations  between  the  velocities  and  accelerations  of  the 


centers  of  mass  links  i  —  1  and  i ,  namely, 

(a)  if  the  i  th  joint  is  R, 

Ci  =  Ci-i  +  (Oi-i  X  8i-i  +  0)i  X  p-  (7.33a) 

Ci  =  C/_i  +  (Oi—i  X  8i—i  +  (Oi-i  X  (cof-i  X  5/_i)  +  (hi  X  p^  + 

(Oi  X  (cOi  X  p^)  (7.33b) 

(b)  if  the  i  th  joint  is 

(Oi  =  (Oi-i  (7.34a) 

(hi  =  (hi-i  (7.34b) 

m  =  8i-i  +  biti  +  Pi  (7.34c) 

V,-  =  (Oi  X  u,-  (7.34d) 

c,-  =  C;_1  +  V/  +  bi^i  (7.34e) 

•  •  • 

c,-  =  c,_i  +  (hi  X  u,-  +  (Oi  X  (v,-  +  Ibiti)  +  biti  (7.34f) 


for  /  =  1,  2,  . . . ,  w,  where  Cq  and  Cq  are  the  velocity  and  acceleration  of  the 
center  of  mass  the  base  link.  If  the  latter  is  an  inertial  frame,  then 

0)0  =  0,  0)0  =  0,  Co  =  0,  Co  =  0  (7.35) 

Expressions  (7.32b)-(7.34f)  are  invariant,  i.e.,  they  hold  in  any  coordinate 
frame,  as  long  as  all  vectors  involved  are  expressed  in  that  frame.  However, 
we  have  vectors  that  are  naturally  expressed  in  the  2^  frame  added  to  vectors 


^The  relations  below  are  made  apparent  with  the  aid  of  Fig.  4.6. 
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expressed  in  the  frame,  and  hence,  a  coordinate  transformation  is  needed. 
This  coordinate  transformation  is  taken  into  account  in  Algorithm  7.4.1, 
whereby  the  logical  variable  R  is  true  if  the  /th  joint  is  R;  otherwise  it  is 
false. 

In  performing  the  foregoing  calculations,  we  need  the  cross  product  of  a  vector 
w  times  e,  in  Ti  coordinates,  the  latter  being  simply  [e,  ]/  =  [0,  0,  1  ]^,  and  hence, 
this  cross  product  reduces  to  [  W2,  —  wi ,  0  ]^,  whereby  w^,  for  /:  =  1 , 2,  3,  are  the  x, 

y ,  and  z  Ti  -components  of  w.  This  cross  product,  then,  requires  no  multiplications 

•  •  • 

and  no  additions.  Likewise,  vectors  biei,  and  biti  take  the  simple  forms 
[0,  0,  bf]^ ,  [0,  0,  bi]^ ,  and  [0,  0,  hi  Y  in  Ti.  Adding  any  of  these  vectors  to  any 
other  vector  in  T  then  requires  one  single  addition. 

Algorithm  7.4.1  (Outward  Recursions): 

read  { Qi  Cq,  coq,  Cq,  coq,  Cq, 

For  i  =  1  to  n  step  1  do 
update  Q/ 
if  R  then 

C;  ^  Qf  (c/-i  +  5/_i)  +  Pi 
^  +  ftci) 

u,_i  ^  (Oi-i  X  5,-1 

V,-  ^  X  Pi 

Ci  ^  Qf  (Ci-l  +  Ui-l)  +  V; 

•  •  • 

Ct)/  Q^-  (Ct)/  —  !  (Oi  — I  X  OiGi  ”1“  OiGi') 

Ci  ^  Qf  (C;-l  +  Mi-I  X  5,-1  +  (Oi-I  X  U;_i) 

+  (Oi  X  Pi  +  (Oi  X  V,- 

else 

U;  ^  QfSi-i+Pi+biet 

Ci  ^  Qfc,-1+U,- 
0)i  ^  Qf<w,-1 

V,-  ^  (Oi  X  u,- 

w,'  ^  bjei 

C;  ^  Qf  C,-l  +  V;  +  W; 

d)i  ^  Qfw,-1 

*  • 

C;  ^  Qfc,-1  +  (bi  X  U;  +  (Oi  X  (v;  +  W;  +  W,)  +  bi^i 
endif 
enddo 

If,  moreover,  we  take  into  account  that  the  cross  product  of  two  arbitrary  vectors 
requires  6M  and  3 A,  we  then  have  the  operation  counts  given  below: 

(a)  If  the  i  th  joint  is  R, 

Qi  requires  4M  and  OA 
Ci  requires  8M  and  10 A 
(Oi  requires  8M  and  5 A 
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Table  7.1  Complexity  of  the 
kinematics  computations 


Item 

M 

A 

{Q/}? 

4n 

0 

WiYi 

Sn 

5n 

{ciY 

20n 

16/1 

lOn 

In 

32n 

2Sn 

Total 

82/1 

66n 

Ci  requires  20M  and  16 A 
00  i  requires  lOM  and  7^4 
Cj  requires  32M  and  28^ 
(b)  If  the  i  th  joint  is  P, 

Qj  requires  4M  and  0^4 
Ci  requires  16M  and  \5A 
coi  requires  8M  and  4^4 
Cf  requires  14M  and  11^ 
(bi  requires  8M  and  4^4 
Ci  requires  20M  and  1 9^4 


The  computational  complexity  for  the  forward  recursions  of  the  kinematics  cal¬ 
culations  for  an  w -revolute  manipulator,  as  pertaining  to  various  algorithms,  are 
summarized  in  Table  7.1.  Note  that  if  some  joints  are  P,  then  these  figures  become 
lower. 


7.4.2  Dynamics  Computations:  Inward  Recursions 

The  free-body  diagram  of  an  intermediate  link  appears  in  Fig.  7.4,  that  of  the  end- 
effector,  or  nth  link,  appearing  in  Fig.  7.5.  Note  that  the  EE  is  acted  upon  by  a  non¬ 
working  constraint  wrench,  exerted  through  the  nth  pair,  and  a  working  wrench;  the 
latter  involves  both  active  and  dissipative  forces  and  moments.  Although  dissipative 
forces  and  moments  are  difficult  to  model  because  of  dry  friction  and  striction,  they 
can  be  readily  incorporated  into  the  dynamics  model,  once  a  suitable  constitutive 
model  for  these  items  is  available.  Since  these  forces  and  moments  depend  only  on 
joint  variables  and  joint  rates,  they  can  be  calculated  once  the  kinematic  variables 
are  known.  For  the  sake  of  simplicity,  dissipative  wrenches  are  not  included  here, 
their  discussion  being  the  subject  of  Sect.  7.8.  Hence,  the  force  and  the  moment  that 
the  (/  —  l)st  link  exerts  on  the  i  th  link  through  the  i  th  joint  only  produce  nonworking 
constraint  and  active  wrenches.  That  is,  for  a  revolute  pair,  one  has 


(7.36) 
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Fig.  7.4  Free-body  diagram  of  the  i  th  link 


n^^link 


Fig.  7.5  Free-body  diagram  of  the  end-effector 


in  which  and  nj  are  the  nonzero  /'/-components  of  the  nonworking  constraint 
moment  exerted  by  the  (/  —  l)st  link  on  the  /  th  link;  obviously,  this  moment  lies  in 
a  plane  perpendicular  to  Z/ ,  whereas  r/  is  the  active  torque  applied  by  the  motor  at 
the  said  joint.  Vector  contains  only  nonworking  constraint  forces. 

For  a  prismatic  pair,  one  has 


nf 

[fn 

II 

a, 

c 

II 

1 

_ 1 

(7.37) 
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where  vector  nf  contains  only  nonworking  constraint  torques,  while  r,  is  now  the 
active  force  exerted  by  the  ith  motor  in  the  Z,  direction,  and  f/  being  the 
nonzero  -components  of  the  nonworking  constraint  force  exerted  by  the  /th  joint 
on  the  ith  link,  which  is  perpendicular  to  the  Z,  axis. 

In  the  algorithm  below,  the  driving  torques  or  forces  { tf  ,  are  computed  via 
vectors  nf  and  ff.  In  fact,  in  the  case  of  a  revolute  pair,  tf  is  simply  the  third 

component  of  nf ;  in  the  case  of  a  prismatic  pair,  r,  is,  accordingly,  the  third 

component  of  ff .  From  Fig.  7.5,  the  Newton-Euler  equations  of  the  end-effector  are 

ff  =  m„c„  -  f  (7.38a) 

nf  =  I„w„  +  «„  X  l„(o„  -  n  +  /)„  X  ff  (7.38b) 

where  f  and  n  are  the  external  force  and  moment,  the  former  being  applied  at  the 
center  of  mass  the  end-effector.  The  Newton-Euler  equations  for  the  remaining 
links  are  derived  based  on  the  free-body  diagram  of  Fig.  7.4,  namely, 

ff  =m, ■<:,•+ ff+i  (7.38c) 

nf  =  +  «,•  X  liCOi  +  nf+i  +  3,-  x  ff^j  +  /),■  x  ff  (7.38d) 

with  8i  defined  as  the  difference  a/  —  Pi  in  Eqs.  (7.32a  and  c). 

Once  the  nf  and  ff  vectors  are  available,  the  actuator  torques  and  forces,  denoted 
by  Ti ,  are  readily  computed.  In  fact,  if  the  /  th  joint  is  a  revolute,  then 

r,-  =  ef  nf  (7.39) 

which  does  not  require  any  further  operations,  for  r,  reduces,  in  this  case,  to 

the  Zi  component  of  vector  nf .  Similarly,  if  the  ith  joint  is  prismatic,  then  the 
corresponding  actuator  force  reduces  to 

T,=efff  (7.40) 

Again,  the  foregoing  relations  are  written  in  invariant  form.  In  order  to  perform 
the  computations  involved,  transformations  that  transfer  coordinates  between  two 
successive  frames  are  required.  Here,  we  have  to  keep  in  mind  that  the  components 
of  a  vector  expressed  in  the  (i  +  l)st  frame  can  be  transferred  to  the  ith  frame  by 
multiplying  the  vector  array  in  (i  +  l)st  coordinates  by  matrix  Q^  .  In  taking  these 
coordinate  transformations  into  account,  we  derive  the  Newton-Euler  algorithm 
from  the  above  equations,  namely. 
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Table  7.2  Complexity  of 
dynamics  computations 


Row  #  M  A 


1 

3 

3 

2 

30 

27 

5 

S{n  —  1) 

4(77  —  1) 

6 

3(71  —  1) 

3(77  —  1) 

7 

44(71  —  1) 

37(«  -  1) 

Total 

55n  -  22 

H 

1 

Algorithm  7.4.2  (Inward  Recursions): 

^  m„Cn  -  f 

nf  ^  !„&)„  +(o„x  l„(o„  -  n  +  /)„  X 

If  R  then 

^  (S^n^ri 
else 

T:n  ^  (Qnfn)z 

For  i=n-l  to  1  step  —1  do 

0?+i  ^  Qz+if/+i 

ff  ^  m,c,+0,+i 

nf  ^  Ii(Oi  +  (Oi  X  li(Oi  +  Pi  X  ff  +  Q,+inf+j  +  5,-  x  0,.+i 
If  R  then 

Ti  ^  (Qi^f)z 

else 

^  (Q(fr)j  enddo 


Note  that,  within  the  do-loop  of  the  foregoing  algorithm,  the  vectors  to  the  left 
of  the  arrow  are  expressed  in  the  ith  frame,  while  and  to  the  right  of  the 
arrow,  are  expressed  in  the  (/  +  l)st  frame. 

In  calculating  the  computational  complexity  of  this  algorithm,  note  that  the  a/  —p^ 
term  is  constant  in  the  (/  +  l)st  frame,  and  hence,  it  is  computed  off-line.  Thus,  its 
computation  need  not  be  accounted  for.  A  summary  of  computational  costs  is  given 
in  Table  7.2  for  an  /7 -revolute  manipulator,  with  the  row  number^  indicating  the  step 
in  Algorithm  7.4.2. 

The  total  numbers  of  multiplications  and  additions  Ad  required  by  the 
foregoing  algorithm  are  readily  obtained,  with  the  result  shown  below: 

Md  =  55n  -  22,  Ad  =  44n  -  14  (7.41) 

In  particular,  for  a  six-revolute  manipulator,  one  has 

n  =  6,  Md  =  308,  Ad  =  250 


"^Only  rows  involving  floating-point  operations  are  counted  here. 


(7.42) 
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Table  7.3  Complexity  of  different  algorithms  for  inverse  dynamics 


Author(s) 

Methods 

Multiplications 

Additions 

Hollerbach  (1980) 

E-L 

412/1  -  277 

320/1  -  201 

Luh  et  al.  (1980) 

N-E 

150/1  -  48 

131/1-48 

Walker  and  Orin  (1982) 

N-E 

137/1-22 

101/1  -  11 

Khalil  et  al.  (1986) 

N-E 

105/1  -  92 

94/1  —  86 

Angeles  et  al.  (1989) 

Kane 

105/1  -  109 

90/1  -  105 

Balafoutis  and  Patel  (1991) 

Tensor 

93/1  -  69 

81/1  —  65 

Li  and  Sankar  (1992) 

E-L 

88/1  —  69 

76/1  —  66 

If  the  kinematics  computations  are  accounted  for,  then  the  Newton-Euler 
algorithm  given  above  for  the  inverse  dynamics  of  /7 -revolute  manipulators  requires 
M  multiplications  and  A  additions,  as  given  below: 

M  =  \31n  -  22,  A  =  l  lOn  -  14  (7.43) 

The  foregoing  number  of  multiplications  is  identical  to  that  reported  by  Walker  and 
Orin  (1982);  however,  the  number  of  additions  is  slightly  higher  than  Walker  and 
Orin’s  figure,  namely,  lOln  —  11. 

Thus,  the  inverse  dynamics  of  a  six-revolute  manipulator  requires  800  multi¬ 
plications  and  646  additions.  These  computations  can  be  performed  in  a  few 
microseconds  using  a  modem  processor.  Clearly,  if  the  aforementioned  algorithms 
are  tailored  to  suit  particular  architectures,  then  they  can  be  further  simplified.  Note 
that,  in  the  presence  of  a  prismatic  pair  in  the  jth  joint,  the  foregoing  complexity 
is  reduced.  In  fact,  if  this  is  the  case,  the  Newton-Euler  equations  for  the  jth  link 
remain  as  in  Eqs.  (7.38c  and  d)  for  the  ith  link,  the  only  difference  appearing  in 
the  implementing  algorithm,  which  is  simplified,  in  light  of  the  results  derived  in 
discussing  the  kinematics  calculations. 

The  incorporation  of  gravity  in  the  Newton-Euler  algorithm  is  done  most 
economically  by  following  the  idea  proposed  by  Luh  et  al.  (1980),  namely,  by 
declaring  that  the  inertial  base  undergoes  an  acceleration  — g,  where  g  denotes  the 
acceleration  of  gravity.  That  is 


Co  =  -g  (7.44) 

the  gravitational  accelerations  thus  propagating  forward  to  the  EE.  A  comparison 
of  various  algorithms  with  regard  to  their  computational  complexity  is  displayed 
in  Table  7.3  for  an  /7 -revolute  manipulator.  Eor  n  =  6,  the  corresponding  figures 
appear  in  Table  7.4. 
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Table  7.4  Complexity  of  different  algorithms  for  inverse  dynamics,  for 
n  =  6 


Author(s) 

Methods 

Multiplications 
(n  =  6) 

Additions 
(n  =  6) 

Hollerbach  (1980) 

E-L 

2195 

1719 

Luh  et  al.  (1980) 

N-E 

852 

738 

Walker  and  Orin  (1982) 

N-E 

800 

595 

Hollerbach  and  Sahar  (1983) 

N-E 

688 

558 

Kane  and  Levinson  (1983) 

Kane 

646 

394 

Khalil  et  al.  (1986) 

N-E 

538 

478 

Angeles  et  al.  (1989) 

Kane 

521 

435 

Balafoutis  and  Patel  (1991) 

Tensor 

489 

420 

Li  and  Sankar  (1992) 

E-L 

459 

390 

7.5  The  Natural  Orthogonal  Complement 

In  simulation  studies,  we  need  to  integrate  the  system  of  ordinary  differential 
equations  (ODE)  describing  the  dynamics  of  a  robotic  mechanical  system.  This 
system  is  known  as  the  mathematical  model  of  the  system  at  hand.  Note  that  the 
Newton-Euler  equations  derived  above  for  a  serial  manipulator  do  not  constitute  the 
mathematical  model  because  we  cannot  use  the  recursive  relations  derived  therein 
to  set  up  the  underlying  ODE  directly.  What  we  need  is  a  model  relating  the  state  of 
the  system  with  its  external  generalized  forces,  of  the  form 

x  =  f(x,u),  x(ro)=xo  (7.45) 

where  x  is  the  state  vector,  u  is  the  input  or  control  vector,  xq  is  the  state  vector 
at  a  certain  time  t^,  and  f(x,  u)  is  a  nonlinear  function  of  x  and  u,  derived  from  the 
dynamics  of  the  system.  The  state  of  a  dynamical  system  is  defined,  in  turn,  as  the 
set  of  variables  that  separate  the  past  from  the  future  of  the  system  (Bryson  and 
Ho  1975).  Thus,  if  we  take  to  as  the  present  time,  we  can  predict  from  Eqs.  (7.45) 
the  future  states  of  the  system  upon  integration  of  the  initial-value  problem  at  hand, 
even  if  we  do  not  know  the  complete  past  history  of  the  system  in  full  detail.  Now, 

if  we  regard  the  vector  0  of  independent  joint  variables  and  its  time-rate  of  change, 

• 

0 ,  as  the  vectors  of  generalized  coordinates  and  generalized  speeds,  then  an  obvious 
definition  of  x  is 


X  = 


(7.46) 


The  n  generalized  coordinates,  then,  define  the  configuration  of  the  system,  while 
their  time-derivatives  determine  its  generalized  momentum,  a  concept  defined  in 
Eq.  (7.19d).  Hence,  knowing  0  and  0  at  to,  we  can  predict  the  future  values  of  these 
variables,  at  ^  >  to,  with  the  aid  of  Eqs.  (7.45). 
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Below  we  will  derive  the  mathematical  model,  Eq.  (7.45),  explicitly,  as  pertaining 
to  serial  manipulators,  in  terms  of  the  kinematic  structure  of  the  system  and  its 
inertial  properties,  i.e.,  the  mass,  center  of  mass  coordinates,  and  inertia  matrix  of 
each  of  its  bodies.  To  this  end,  we  first  write  the  underlying  system  of  uncoupled 
Newton-Euler  equations  for  each  link.  We  have  n  -\-  \  links  numbered  from  0  to 
n,  which  are  coupled  by  n  kinematic  pairs.  Moreover,  the  base  link  0  need  not 
be  an  inertial  frame;  if  it  is  noninertial,  then  the  force  and  moment  exerted  by  the 
environment  upon  it  must  be  known.  Eor  ease  of  presentation,  we  will  assume  in 
this  section  that  the  base  frame  is  inertial,  the  modifications  needed  to  handle  a 
noninertial  base  frame  to  be  introduced  in  Sect.  7.5.2. 

We  now  recall  the  Newton-Euler  equations  of  the  ith  body  in  six-dimensional 
form,  Eqs.  (7.5c),  which  we  reproduce  below  for  quick  reference: 

M,t/  =  +  w,^  +  wf ,  i  =  (7.47) 

Eurthermore,  the  definitions  of  Eqs.  (7.13b  and  c)  are  recalled.  Apparently,  M  and 
W  are  now  6n  x  6n  matrices,  while  t,  w^,  and  are  all  6 -dimensional 
vectors.  Then,  the  foregoing  6n  scalar  equations  for  the  n  moving  links  take  the 
simple  form 


Mt  =  -WMt  +  (7.48) 

in  which  has  been  decomposed  into  its  active,  gravitational,  and  dissipative 
parts  w^,  w^,  and  w^,  respectively.  Now,  since  gravity  acts  at  the  center  of  mass  of 
a  body,  the  gravity  wrench  wp  acting  on  the  i  th  link  takes  the  form 

(7.49) 

The  mathematical  model  displayed  in  Eq.  (7.48)  represents  the  6n  uncoupled 
Newton-Euler  equations  of  the  overall  manipulator.  The  following  step  of  this 
derivation  consists  in  reducing  the  uncoupled  system  of  equations  to  a  set  of 
n  coupled  equations  of  motion.  To  this  end,  the  coupling  between  every  two 
consecutive  links  is  represented  as  a  linear  homogeneous  system  of  algebraic 
equations  on  the  link  twists.  Moreover,  we  note  that  all  kinematic  pairs  allow  a 
relative  one-degree-of-freedom  motion  between  the  coupled  bodies.  We  can  then 
express  the  kinematic  constraints  of  the  system  in  linear  homogeneous  form  in  the 
6  w -dimensional  vector  of  manipulator  twist,  namely. 


wf  = 


0 

rriig 


Kt  =  0  (7.50) 

with  K  being  a  6/2  x  6/7  matrix,  to  be  derived  in  Sect.  7.5.1.  What  is  important  to  note 
at  the  moment  is  that  the  kinematic  constraint  equations,  or  constraint  equations, 
for  brevity,  Eqs.  (7.50),  consist  of  a  system  of  6/7  scalar  equations,  i.e.,  six  scalar 
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equations  for  each  joint,  for  the  manipulator  at  hand  has  n  joints.  Moreover,  when 
the  system  is  in  motion,  t  is  different  from  zero,  and  hence,  matrix  K  is  bound  to  be 
singular.  In  fact,  the  dimension  of  the  null  space  of  K,  termed  its  nullity,  is  exactly 
equal  to  n,  the  degree  of  freedom  of  the  manipulator.  That  is,  not  every  possible  6n- 
dimensional  twist  is  feasible',  to  be  so,  a  twist  must  lie  in  the  null  space  of  matrix  K. 
Furthermore,  since  the  nonworking  constraint  wrench  produces  no  work  on  the 
manipulator,  its  sole  function  being  to  keep  the  links  together,  the  power  developed 
by  this  wrench  on  t,  for  any  feasible  motion  of  the  manipulator,  is  zero,  i.e., 

Vw'^=0  (7.51) 

On  the  other  hand,  if  the  two  sides  of  Eq.  (7.50)  are  transposed  and  then  multiplied 
by  a  6/2 -dimensional  vector  A,  one  has 


=  0  (7.52) 

Upon  comparing  Eqs.  (7.51)  and  (7.52),  it  is  apparent  that  is  of  the  form 

=  K^A  (7.53) 

More  formally,  the  scalar  product  of  and  t,  as  stated  by  Eq.  (7.51),  vanishes,  and 
hence,  t  lies  in  the  null  space  of  K,  as  stated  by  Eq.  (7.50).  This  means  that  lies 
in  the  range  of  K^,  as  stated  in  Eq.  (7.53).  The  following  step  will  be  to  represent  t 
as  a  linear  transformation  of  the  independent  generalized  speeds,  i.e.,  as 


t  =  TO  (7.54) 

with  T  defined  as  a  6/2  x  //  matrix  that  can  be  fairly  termed  the  twist-shaping  matrix. 
Moreover,  the  above  mapping  will  be  referred  to  as  the  twist-shape  relations. 
The  derivation  of  expressions  for  matrices  K  and  T  will  be  described  in  detail  in 
Sect.  7.5.1  below.  Now,  upon  substitution  of  Eq.  (7.54)  into  Eq.  (7.50),  we  obtain 

KT^  =  0  (7.55a) 

Furthermore,  since  the  degree  of  freedom  of  the  manipulator  is  n,  the  n 
generalized  speeds  {  Of  can  be  assigned  arbitrarily.  However,  while  doing  this, 
Eq.  (7.55a)  has  to  hold.  Thus,  the  only  possibility  for  this  to  happen  is  that  the 
product  KT  vanish,  i.e.. 


KT  =  O  (7.55b) 

where  O  denotes  the  6n  x  n  zero  matrix.  The  above  equation  states  that  T  is 
an  orthogonal  complement  of  K.  Because  of  the  particular  form  of  choosing  this 
complement — see  Eq.  (7.54) — we  refer  to  T  as  the  natural  orthogonal  complement 
of  K  (Angeles  and  Lee  1988). 
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In  the  final  step  of  this  method,  t  of  Eq.  (7.48)  is  obtained  from  Eq.  (7.54), 
namely. 


i  =  T0  ^TO  (7.56) 

Eurther,  both  sides  of  the  uncoupled  equations  (7.48)  are  multiplied  from  the  left  by 
T^,  the  effect  being  that  the  term  is  eliminated.  Indeed,  if  is  expressed  as 
appearing  in  Eq.  (7.53),  we  have 

=  (KT)^A 

However,  by  virtue  of  Eq.  (7.55b),  the  matrix  coefficient  of  X  in  the  foregoing 
expression  vanishes,  thereby  obtaining  n  independent  equations  free  of  nonworking 
constraint  wrenches.  These  are  nothing  but  the  governing  equations  of  the  manipu¬ 
lator,  namely. 


iO  =  -T^(Mt  +  WMT)^  +  +  w"  +  w^)  (7.57) 

where  I,  the  positive  definite  n  xn  generalized  inertia  matrix  of  the  manipulator,  is 
defined  as 


I  =  T^MT  (7.58) 

which  is  identical  to  the  inertia  matrix  derived  using  the  Euler-Lagrange  equations, 
with  0  as  the  vector  of  generalized  coordinates.  Now,  we  let  r  and  8  denote  the 

-dimensional  vectors  of  active  and  dissipative  generalized  force.  Moreover,  we  let 

•  • 

C(^,  0)0  be  the  -dimensional  vector  of  quadratic  terms  of  inertia  force.  These 
items  are  defined  as 


TT  A  j?  _  ^~w^T  D  _  ^~w^T  G 

"w  ,  o=T  w  ,  y  =  T  w  , 

C{0,0)  =  T^MT  +  T^WMT  (7.59) 

Clearly,  the  sum  r  -\-  8  produces  0^,  the  generalized  force  defined  in  Eq.  (7.11). 
Thus,  the  Euler-Lagrange  equations  of  the  system  take  the  form 

=  -C^  +  T  +  5  -h  y  (7.60) 

If,  moreover,  a  static  wrench  acts  onto  the  end-effector,  with  the  force  applied 
at  the  operation  point,  then  its  effect  onto  the  above  model  is  taken  into  account  as 
indicated  in  Eq.  (5.50).  Thus,  a  term  is  added  to  the  right-hand  side  of  the 

above  model: 


10  =  -C0  +  T  +S  +  y+ 


(7.61) 
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As  a  matter  of  fact,  5  is  defined  in  Eq.  (7.59)  only  for  conceptual  reasons. 
In  practice,  this  term  is  most  readily  calculated  once  a  dissipation  function  in  terms 
of  the  generalized  coordinates  and  generalized  speeds  is  available,  as  described  in 
Sect.  7.8.  Thus,  8  is  computed  as 


8 


9  A 

do 


(7.62) 


It  is  pointed  out  that  the  first  term  of  the  right-hand  side  of  Eq.  (7.60)  is  quadratic 

•  • 

in  0  because  matrix  C,  defined  in  Eq.  (7.59),  is  linear  in  ^ .  In  fact,  the  first  term  of 

•  • 

that  expression  is  linear  in  a  factor  T  that  is,  in  turn,  linear  in  0 .  Moreover,  the  second 
term  of  the  same  expression  is  linear  in  W,  which  is  linear  in  0  as  well.  However,  C 
is  nonlinear  in  0 .  Because  of  the  quadratic  nature  of  that  term,  it  is  popularly  known 
as  the  vector  of  Coriolis  and  centrifugal  forces,  whereas  the  left-hand  side  of  that 
equation  is  given  the  name  of  vector  of  inertia  forces.  Properly  speaking,  both  the 
left-hand  side  and  the  first  term  of  the  right-hand  side  of  Eq.  (7.60)  arise  from  inertia 
forces. 

Example  7.5.1  (A  Minimum-Time  Trajectory).  A  pick-and-place  operation  is  to  be 
performed  with  a  w-axis  manipulator  in  the  shortest  possible  time.  Moreover,  the 
maneuver  is  defined  so  that  the  w -dimensional  vector  of  joint  variables  is  given  by  a 
common  shape  function  ^(x),  with  0  <  x  <  1  and  0  <  ^  <  1,  which  is  prescribed. 
Thus,  for  a  fixed  w -dimensional  vector  the  time-history  of  the  joint- variable 
vector,  ^  (^),  is  given  by 


(9(0  =  6>o  +  ^(^^j  Al9,  0<t<T 

with  T  defined  as  the  time  taken  by  the  maneuver,  while  Oq  and  Oq  +  are 
the  values  of  the  joint- variable  vector  at  the  pick-  and  the  place-postures  of  the 
manipulator,  respectively.  These  vectors  are  computed  upon  solving  the  inverse- 
displacement  problem,  as  explained  in  Chap.  4.  Eurthermore,  the  load-carrying 
capacity  of  the  manipulator  is  specified  in  terms  of  the  maximum  torques  delivered 
by  the  motors,  namely. 


r,- 


< 


-Ci 


for  i  =  \, . . .  ,n 


where  the  constant  values  r/  are  to  be  found  from  data  supplied  by  the  manufacturer. 
In  order  to  keep  the  analysis  simple,  we  neglect  power  loses  in  this  example.  Eind 
the  minimum  time  in  which  the  maneuver  can  take  place. 

Solution:  Let  us  first  calculate  the  vector  of  joint-rates  and  its  time-derivative: 

o{t)  =  2/(x)a0,  0{t)  =  Es"{x)^e,  x  =  j 
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Now  we  substitute  the  above  values  into  the  mathematical  model  of  Eq.  (7.60),  with 
8(t)  =  0  and  y(t)  =  0,  thereby  obtaining 

T  =  i{e)e  +  c(e,e)e  =  — r[/'(x)i(x)A0  +  s'^(x)C(x,Ae)Ae]  =  -rfCx) 

with  f(x)  defined,  of  course,  as 

f(x)  =  [I(v)^''(v)  +  C(x)^'^(v)] 

the  \/T^  factor  in  the  term  of  Coriolis  and  centrifugal  forces  stemming  from  the 

•  • 

quadratic  nature  of  the  C(0 ,0)0  term.  What  we  now  have  is  the  vector  of  motor 
torques,  t,  expressed  as  a  function  of  the  scalar  argument  x.  Now,  let  fi(x)  be  the 
ith  component  of  vector  f(x),  and 

Fi  =  msix{\fi(x)\},  for  i  = 

X 

We  would  then  like  to  have  each  value  Fi  produce  the  maximum  available  torque 
Ti ,  namely. 


and  hence,  for  each  joint  we  have  a  value  Ti  of  T  given  by 


Obviously,  the  minimum  value  sought,  Tmin,  is  nothing  but  the  maximum  of  the 
foregoing  values,  i.e., 

Tmin  =  max{7;}'; 


thereby  completing  the  solution. 


7.5.1  Derivation  of  Constraint  Equations  and  Twist-Shape 
Relations 

In  order  to  illustrate  the  general  ideas  behind  the  method  of  the  natural  orthogonal 
complement,  we  derive  below  the  underlying  kinematic  constraint  equations  and 
the  twist-shape  relations.  We  first  note,  from  Eq.  (7.25a),  that  the  relative  angular 
velocity  of  the  / th  link  with  respect  to  the  (/  —  l)st  link,  o),  —  is  .  Thus,  if 
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matrix  is  defined  as  the  cross-product  matrix  of  vector  e^,  then,  the  angular 
velocities  of  two  successive  links  obey  a  simple  relation,  namely, 

-  w,_i)  =  0  (7.63) 

Furthermore,  we  rewrite  now  Eq.  (7.33a)  in  the  form 

ti  —  C/_i  +  R/cu/  +  =  0  (7.64) 

where  and  R/  are  defined  as  the  cross-product  matrices  of  vectors  5/,  defined  in 
Sect.  7.4.1  as  a,  —p^ ,  and  p^ ,  respectively.  In  particular,  when  the  first  link  is  inertial, 
Eqs.  (7.63  and  b),  as  pertaining  to  the  first  link,  reduce  to 

Ei(Oi=0  (7.65a) 

ci+Ricui=0  (7.65b) 

Now,  Eqs.  (7.63)  and  (7.64),  as  well  as  their  counterparts  for  i  =  I,  Eqs.  (7.65a 
and  b),  are  further  expressed  in  terms  of  the  link  twists,  thereby  producing  the 
constraints  below: 


Kiiti  —  0 

— ll?  — 1  3“  Rn't?  —  ^  \  ,  .  .  .  ,  Tl 

with  Kii  and  K,y ,  for  /  =  2, . . .  ,n  and  j  =  /  —  1 ,  / ,  defined  as 


(7.66a) 

(7.66b) 


K 


11 


El  O 

Ri  1 


-E,  O 

D, -i  -1 

E,  O' 

Rz  1 


(7.67a) 

(7.67b) 

(7.67c) 


where  1  and  O  denote  the  3x3  identity  and  zero  matrices,  respectively.  Furthermore, 
from  Eqs.  (7.66a  and  b)  and  (7.67a-c),  it  is  apparent  that  matrix  K  appearing  in 
Eq.  (7.55b)  takes  on  the  form 


■Ku 

Oe 

Os 

Os 

Os' 

K21 

K22 

Os  •  •  • 

•  • 

Os 

Os 

Oe 

Os 

•  *  • 

06  •  •  •  K„_p„_i 

Os 

-Oe 

Os 

06  •  •  •  K/t  1 

Rzzzz- 

(7.68) 


with  06  denoting  the  6x6  zero  matrix. 
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Fig.  7.6  Kinematic  subchain  comprising  links  7,7+!...,/ 


Further,  the  link- twists  are  expressed  as  linear  combinations  of  the  joint-rate 
vector  0 .  To  this  end,  we  define  the  6xn  partial  Jacobian  it  as  the  matrix  mapping 
the  joint-rate  vector  0  into  the  twist  it  of  that  link,  i.e., 

JiO  =ti  (7.69) 


whose  7  th  column,  t/7,  is  given,  for  /  ,  7  =  1,  2,  . . . ,  /i,  by 


V 


ej  X  ^ij 

o' 

0 


if  7  <C 
otherwise. 


(7.70) 


with  r/7  illustrated  in  Fig.  7.6  and  defined,  for  /,  7  =  1, . . . ,  /7,  as 


ay  +  ay+1  H - h  a,_i  +  /o,-,  if  j  <  i; 

0,  otherwise. 


(7.71) 


It  is  noteworthy  that,  for  a  given  i  and  a  given  7  <  /  ,  a  submanipulator  of  i  —  (7  — 
1)  axes  is  obtained.  The  {r/7  Yj=i  vectors  are  the  counterparts  of  the  {r^ vectors  of 
Sect.  5.2 

We  can  thus  readily  express  the  twist  t,  of  the  i  th  link  as  a  linear  combination  of 
the  first  i  joint  rates,  namely. 


t;  —  ^lt/1  +  +  •  •  •  +  ft  tn' »  i  —  1,  .  .  .  , 


(7.72) 
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and  hence,  matrix  T  of  Eq.  (7.54)  takes  the  form 


til  0  0 

hi  hi  ' "  0 


_t«l  hi 


t 


nn. 


(1.13) 


As  a  matter  of  verification,  one  can  readily  prove  that  the  product  of  matrix  T, 
as  given  by  Eq.  (7.73),  by  matrix  K,  as  given  by  Eq.  (7.68),  vanishes,  and  hence, 
relation  (7.55b)  holds. 

The  kinematic  constraint  equations  on  the  twists,  for  the  case  in  which  the  ith 
joint  is  prismatic,  are  derived  likewise.  In  this  case,  we  use  Eqs.  (7.34a  and  e),  with 
the  latter  rewritten  more  conveniently  for  our  purposes,  namely, 

(Oi  =  (Oi-i  (7.74a) 

Cj  =  Cj-i  +  (Oj-i  X  (5/_i  +  Pi  +  bjej)  +  bfej  (7.74b) 

We  now  introduce  one  further  definition: 

R;  ^  D;_i  +  R,  (7.75) 

where  is  the  cross-product  matrix  of  vector  5/_i,  defined  in  Sect.  7.4.1  as 
d/_i  —  Pi-i,  while  Ri  is  the  cross-product  matrix  of  Pi  +  btet.  Hence,  Eq.  (7.74b) 
can  be  rewritten  as 


Ci  -  Ci-i  +  R-(Oi  -  bi^i  =  0  (7.76) 

Upon  multiplication  of  both  sides  of  Eq.  (7.76)  by  ,  the  term  in  bt  cancels,  and  we 
obtain 


Ei(Ci  -Ci-i  +R'«i)  =0 


(7.77) 


Hence,  Eqs.  (7.74a)  and  (7.77)  can  now  be  regrouped  in  a  single  six-dimensional 
linear  homogeneous  equation  in  the  twists,  namely. 


=  0 


the  associated  matrices  being  defined  below: 


-1  O 
O  -E, 


1  O 

E/r;  e,- 


(7.78) 


(7.79a) 

(7.79b) 
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with  1  and  O  defined  already  as  the  3x3  identity  and  zero  matrices,  respectively. 
If  the  first  joint  is  prismatic,  then  the  corresponding  constraint  equation  takes  on  the 
form 


K'liti  =  0 


with  defined  as 


1  O 

OEi 


(7.80) 


(7.81) 


Furthermore,  if  the  kth  pair  is  prismatic  and  !</:</,  then  the  twist  t,  of  the 
i  th  link  changes  to 

ii  =  Oiiii  +  •  •  •  +  ,  i  =  I, ...  ,n  (7.82) 


where  is  defined  as 


0 


(7.83) 


In  order  to  set  up  Eq.  (7.60),  then  all  we  now  need  is  T,  which  is  computed 
below.  Two  cases  will  be  distinguished  again,  namely,  whether  the  joint  at  hand  is  a 
revolute  or  a  prismatic  pair.  In  the  first  case,  from  Eq.  (7.70)  one  readily  derives,  for 
i ,  j  1 , 2 , . . . ,  , 


(Oj  X  ey 


V 


(o)j  X  ey)  X  r,y  +  ey  X  r,y 


9 


0 

0 


9 


if;  </: 

otherwise 


where,  from  Eq.  (7.71), 


(7.84) 


Vij  =  (Oj  xsij  - h  (Oi-i  X  a,_i  +  (Oi  X  p-  (7.85) 

On  the  other  hand,  if  the  kth  pair  is  prismatic  and  I  <  k  <  i,  then  from 
Eq.  (7.83),  the  time-rate  of  change  of  becomes 


0 

(Ok  X 


(7.86) 


thereby  completing  the  desired  derivations. 


316 


7  Dynamics  of  Serial  Robotic  Manipulators 


Note  that  the  natural  orthogonal  complement  can  also  be  used  for  the  inverse 
dynamics  calculations.  In  this  case,  if  the  manipulator  is  subjected  to  a  gravity 
field,  then  the  twist-rate  of  the  first  link  will  have  to  be  modified  by  adding  a 
nonhomogeneous  term  to  it,  thereby  accounting  for  the  gravity- acceleration  terms. 
This  issue  is  discussed  in  Sect.  7.7. 


7.5.2  Noninertial  Base  Link 

Noninertial  bases  occur  in  space  applications,  e.g.,  in  the  case  of  a  manipulator 
mounted  on  a  space  platform  or  on  the  space  shuttle.  A  noninertial  base  can  be 
readily  handled  with  the  use  of  the  natural  orthogonal  complement,  as  discussed  in 
this  subsection.  Since  the  base  is  free  of  attachments  to  an  inertial  frame,  we  have 
to  add  its  six  degrees  of  freedom  (dof)  to  the  n  dof  of  the  rest  of  the  manipulator. 
Correspondingly,  t,  w^,  w^,  and  now  become  6{n  -\-  1) -dimensional  vectors.  In 
particular,  t  takes  the  form 


t  =  [tj  t[  . . .  tj]^  (7.87) 

with  to  defined  as  the  twist  of  the  base.  Furthermore,  the  vector  of  independent 
generalized  speeds,  0 ,  is  now  of  dimension  /7 +6,  its  first  six  components  being  those 
of  to,  the  other  n  remaining  as  in  the  previous  case.  Thus,  0  has  the  components 
shown  below: 


e^[tl  01...  4]^ 

Correspondingly,  T  becomes  a  6(/i  +  1)  x  (/7  +  6)  matrix,  namely. 


1  O 

O'  T 


(7.88) 


(7.89) 


where  1  is  the  6x6  identity  matrix,  O  denotes  the  6  x  n  zero  matrix.  O'  represents 
the  6n  X  6  zero  matrix,  and  T'  is  the  6n  x  n  matrix  defined  in  Eq.  (7.73)  as  T. 
Otherwise,  the  model  remains  as  in  the  case  of  an  inertial  base. 

A  word  of  caution  is  in  order  here.  Because  of  the  presence  of  the  twist  vector  to 
in  the  definition  of  the  vector  of  generalized  speeds  above,  the  latter  cannot,  properly 
speaking,  be  regarded  as  a  time-derivative.  Indeed,  as  studied  in  Chap.  3,  the  angular 
velocity  appearing  in  the  twist  vector  is  not  a  time-derivative.  Hence,  the  vector  of 
independent  generalized  speeds  defined  in  Eq.  (7.88)  is  represented  instead  by  v, 
which  does  not  imply  a  time-derivative,  namely, 

V  =  [tj  9i  •••4]^ 


(7.90) 
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7.6  Manipulator  Forward  Dynamics 

Forward  dynamics  is  needed  either  for  purposes  of  simulation  or  for  the  model- 
based  control  of  manipulators  (Craig  1989),  and  hence,  a  fast  calculation  of  the 
joint- variable  time-histories  0(t)  is  needed.  These  time-histories  are  calculated  from 
the  model  displayed  in  Eq.  (7.61),  reproduced  below  for  quick  reference,  in  terms 
of  vector  0(t),  i.e.. 


le  =  -ciej)e  +  Tit)  +  8(9j)  +  yie)  +  fw'^  (7.91) 

•  • 

Clearly,  what  is  at  stake  here  is  the  calculation  of  0  from  the  foregoing  model. 
Indeed,  the  right-hand  side  of  Eq.  (7.91)  can  be  calculated  with  the  aid  of  the 

Newton-Euler  recursive  algorithm,  as  we  will  describe  below,  and  needs  no  further 

•  • 

discussion  for  the  time  being.  Now,  the  calculation  of  0  from  Eq.  (7.91)  is  similar  to 
the  calculation  of  0  from  the  relation  between  the  joint-rates  and  the  twist,  derived 
in  Sect.  5.2.  From  the  discussion  in  that  section,  such  calculations  take  a  number 
of  floating-point  operations,  or  flops,  that  is  proportional  to  n^,  and  is  thus  said  to 
have  a  complexity  of  0(n^) — read  “order  In  real-time  calculations,  we  would 
like  to  have  a  computational  scheme  of  0(n).  In  attempting  to  derive  such  schemes. 
Walker  and  Orin  (1982)  proposed  a  procedure  that  they  called  the  composite  rigid- 
body  method,  whereby  the  number  of  flops  is  minimized  by  cleverly  calculating 

I(^)  and  the  right-hand  side  of  Eq.  (7.91)  by  means  of  the  recursive  Newton- 

_  •  • 

Euler  algorithm.  In  their  effort,  they  produced  an  0(jr)  algorithm  to  calculate  0 . 
Thereafter,  Featherstone  (1983)  proposed  an  0{n)  algorithm  that  is  based,  however, 
on  the  assumption  that  Coriolis  and  centrifugal  forces  are  negligible.  The  same 
author  reported  an  improvement  to  the  aforementioned  algorithm,  namely,  the 
articulated-body  method,  that  takes  into  account  Coriolis  and  centrifugal  forces 
(Featherstone  1987.)  The  outcome,  for  a  /i-revolute  manipulator,  is  an  algorithm 
requiring  300w  —  267  multiplications  and  21 9n  —  259  additions.  For  n  =  6,  these 
figures  yield  1,533  multiplications  and  1,415  additions.  Li  (1989)  reported  an  0(n^) 
algorithm  leading  to  783  multiplications  and  670  additions. 

In  this  subsection,  we  illustrate  the  application  of  the  method  of  the  natural 
orthogonal  complement  to  the  modeling  of  an  /i-axis  serial  manipulator  for  purposes 
of  simulation.  While  this  algorithm  gives  an  0(n^)  complexity,  its  derivation 
is  straightforward  and  gives,  for  a  six-axis  manipulator,  a  computational  cost 
similar  to  that  of  Featherstone’ s,  namely,  1,596  multiplications  and  1,263  additions. 
Moreover,  a  clever  definition  of  coordinate  frames  leads  to  even  lower  figures,  i.e., 
1,353  multiplications  and  1,165  additions,  as  reported  by  Angeles  (1988).  Further 
developments  on  robot  dynamics  using  the  natural  orthogonal  complement  have 
been  reported  by  Saha  (1997,  1999,  2008),  who  proposed  the  decoupled  natural 
orthogonal  complement  as  a  means  to  enable  the  real-time  inversion  of  the  mass 
matrix. 

The  manipulator  at  hand  is  assumed  to  be  constituted  by  n  moving  links  coupled 
by  n  kinematic  pairs  of  the  revolute  or  prismatic  types.  Again,  for  brevity,  the 
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base  link  is  assumed  to  be  inertial,  noninertial  bases  being  readily  incorporated  as 
described  in  Sect.  7.5.2.  For  the  sake  of  conciseness,  we  will  henceforth  consider 
only  manipulators  mounted  on  an  inertial  base.  Moreover,  we  assume  that  the 
generalized  coordinates  0  and  the  generalized  speeds  0  are  known  at  an  instant 
tk,  along  with  the  driving  torque  r(t),  for  t  >  4,  and  of  course,  the  DH  and 

the  inertial  parameters  of  the  manipulator  are  assumed  to  be  known  as  well. 

•  • 

Based  on  the  foregoing  information,  then,  0  is  evaluated  at  4  and,  with  a  suitable 

integration  scheme,  the  values  of  0  and  0  are  determined  at  instant  4+i.  Obviously, 

•  • 

the  governing  equation  (7.60)  enables  us  to  solve  for  ^(4).  This  requires,  of 

course,  the  inversion  of  the  n  n  matrix  of  generalized  inertia  I.  Since  the  said 

•  • 

matrix  is  positive-definite,  solving  for  0  from  Eq.  (7.60)  can  be  done  economically 
using  the  Chole sky -decomposition  algorithm  (Dahlquist  and  Bjorck  1974).  The  sole 
remaining  task  is,  then,  the  computation  of  I,  the  quadratic  inertia  term  C^,  and 
the  dissipative  torque  8 .  The  last  of  these  is  dependent  on  the  manipulator  and  the 
constitutive  model  adopted  for  the  representation  of  viscous  and  Coulomb  friction 
forces  and  will  not  be  considered  at  this  stage.  Models  for  dissipative  forces  will  be 
studied  in  Sect.  7.8.  Thus,  the  discussion  below  will  focus  on  the  computation  of  I 
and  appearing  in  the  mathematical  model  of  Eq.  (7.91). 

Next,  the  6n  x  6n  matrix  M  is  factored  as 

M  =  H^H  (7.92) 


which  is  possible  because  M  is  at  least  positive-semidefinite.  In  particular,  for 
manipulators  of  the  type  at  hand,  M  is  positive-definite  if  no  link-mass  is  neglected. 
Moreover,  due  to  the  diagonal-block  structure  of  this  matrix,  its  factoring  is 
straightforward.  In  fact,  H  is  given  simply  by 


H  =  diag(Hi, . . .  ,H„) 


each  6x6  block  H/  of  Eq.  (7.93)  being  given,  in  turn,  as 


N,  O 
O  nil 


(7.93) 


(7.94) 


with  1  and  O  defined  as  the  3x3  identity  and  zero  matrices,  respectively.  We  thus 
have 


M,-  =  Hf  H,-  (7.95) 

Furthermore,  N,  can  be  obtained  from  the  Cholesky  decomposition  of  1/ ,  while  nt 
is  the  positive  square  root  of  m, ,  i.e.. 


I,=NfN,-,  mi=nj 


(7.96) 
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Now,  since  each  6  x  6  M/  block  is  constant  in  body-fixed  coordinates,  the  above 
factoring  can  be  done  off-line.  From  the  foregoing  definitions,  then,  the  nxn  matrix 
of  generalized  inertia  I  can  now  be  expressed  as 

I  =  P^P  (7.97) 

where  P  is  defined,  in  turn,  as  the  6n  xn  matrix  given  below: 

P  =  HT  (7.98) 

The  computation  of  P  is  now  discussed.  If  we  recall  the  structure  of  T  from 
Eq.  (7.73)  and  that  of  H  from  Eq.  (7.93),  along  with  the  definition  of  P,  Eq.  (7.98), 
we  readily  obtain 


Hitii  0  0 

Pii  0  •••  0 

H2t21  H2t22  •  •  •  0 

• 

P21  P22  •  0 

• 

•  •  *  • 

•  •  *  • 

-PtzI  P«2  ‘  ‘  *  P««_ 

(7.99) 


with  0  denoting  the  six-dimensional  zero  vector.  Moreover,  each  of  the  above 
nontrivial  six-dimensional  arrays  is  given  as 


Pz7  —  H;  ty  — 


N/ey 


«,ey  X  r,-; 


if  the  7  th  joint  is  R 


ij 


(7.100) 


0 


if  the  7  th  joint  is  P 


Thus,  the  (/,  j)  entry  of  I  is  computed  as  the  sum  of  the  inner  products  of  the 
(k,  i)  and  the  (k,  j)  blocks  of  P,  for  k  =  7,  •  •  • ,  n,  i.e.. 


k=j 


(7.101) 


with  both  and  p^y  expressed  in  7^^+ 1 -coordinates,  i.e.,  in  kth-link  coordinates. 
Now,  the  Cholesky  decomposition  of  I  can  be  expressed  as 

I  =  L^L  (7.102) 


where  hisannxn  lower- triangular  matrix  with  positive  diagonal  entries.  Moreover, 
Eq.  (7.91)  is  now  rewritten  as 


=  -(CO  -  -  y)  +  5  +  T 


(7.103) 
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If  we  now  recall  Eq.  (7.91),  it  is  apparent  that  the  term  inside  the  parentheses  in  the 
right-hand  side  of  the  above  equation  is  nothing  but  the  torque  required  to  produce 
the  motion  prescribed  by  the  current  values  of  0  and  0 ,  in  the  absence  of  dissipative 
wrenches  and  with  zero  joint  accelerations,  when  the  manipulator  is  acted  upon  by  a 
static  wrench  w^.  That  is,  if  we  call  r  the  torque  r  of  Eq.  (7.91)  under  the  foregoing 
conditions,  then 


y  — 


=  r 


(7.104) 


which  is  most  efficiently  computed  from  inverse  dynamics,  using  the  recursive 

Newton-Euler  algorithm,  as  described  in  Sect.  7.4.  Now  Eq.  (7.102)  is  solved  for 

•  • 

0  in  two  steps,  namely. 


L^x  =  —T  r  8  (7.105a) 

=  X  (7.105b) 

In  the  above  equations,  then,  x  is  first  computed  from  Eq.  (7.105a)  by  backward 

•  • 

substitution.  With  x  known,  0  is  computed  from  Eq.  (7.105b)  by  forward  substi- 

•  • 

tution,  thereby  completing  the  computation  of  0 .  The  complexity  of  the  foregoing 

algorithm  is  discussed  in  Sect.  7.6.2. 

•  • 

Alternatively,  0  can  be  calculated  in  two  steps  from  two  linear  systems  of 

equations,  the  first  one  underdetermined,  the  second  overdetermined.  Indeed,  if  we 

•  • 

let  the  product  be  denoted  by  y,  then  the  dynamics  model  of  the  manipulator, 
Eq.  (7.60),  along  with  the  factoring  of  Eq.  (7.97),  leads  to 

P^y  =  — T  +  T  +  5  (7.106a) 

P^  =  y  (7.106b) 

Thus,  in  the  above  equations,  y  is  calculated  first  as  the  minimum-norm  solution 

•  • 

of  Eq.  (7.106a);  then,  the  desired  value  of  0  is  calculated  as  the  least-square  approx¬ 
imation  of  Eq.  (7.106b).  These  two  solutions  are  computed  most  efficiently  using  an 
orthogonalization  algorithm  that  reduces  matrix  P  to  upper-triangular  form  (Golub 
and  Van  Loan  1989).  A  straightforward  calculation  based  on  the  explicit  calculation 
of  the  generalized  inverses  involved  is  not  recommended,  because  of  the  frequent 
numerical  ill-conditioning  incurred.  Two  orthogonalization  procedures,  one  based 
on  Householder  reflections,  the  other  on  the  Gram-Schmidt  procedure,  for  the 
computation  of  both  the  least-square  approximation  of  an  overdetermined  system 
of  equations  and  the  minimum-norm  solution  of  its  underdetermined  counterpart 
are  outlined  in  Appendix  B. 

The  complexity  of  the  foregoing  calculations  is  discussed  in  Sect.  7.6.2,  based 
on  the  Cholesky  decomposition  of  the  generalized  inertia  matrix,  details  on  the 
alternative  approach  being  available  elsewhere  (Angeles  and  Ma  1988). 
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Fig.  7.7  Mass-center 
locations  of  the  robot  of 
Fig.  4.23 


Example  7.6.1  (Dynamics  of  a  Spatial  Three -Revolute  Robot).  The  robot  of 
Fig.  4.17  is  reproduced  in  Fig.  7.7,  in  a  form  that  is  kinematically  equivalent  to 
the  sketch  of  that  figure,  but  more  suitable  for  the  purposes  of  this  example.  For 
this  robot,  (a)  find  its  inertia  matrix  at  the  configuration  depicted  in  that  figure; 

(b)  find  the  time-rate  of  change  of  the  inertia  matrix  under  a  maneuver  whereby 

•  •  •  ••  ••  •• 

=62  =  03  =  p  s~^  and  61  =  62  =  03=0;  and  (i  i  i)  under  the  same  maneuver, 
find  the  centrifugal  and  Coriolis  terms  of  its  governing  equation.  Furthermore, 
assume  that  all  links  are  identical  and  dynamically  isotropic.  What  we  mean  by 
“dynamically  isotropic”  is  that  the  moment  of  inertia  of  all  three  links  about  their 
centers  of  mass  are  proportional  to  the  3x3  identity  matrix,  the  proportionality 
factor  being  I .  Moreover,  all  three  links  are  designed  so  that  the  center  of  mass  of 
each  is  located  as  shown  in  Fig.  7.7. 

Solution: 

(a)  Henceforth,  we  represent  all  vectors  and  matrices  with  respect  to  the  -frame 
of  Fig.  7.7,  while  denoting  by  i,  j,  and  k  the  unit  vectors  parallel  to  the  Xi,  Yi, 
and  Zi  axes,  respectively.  Under  these  conditions,  we  have,  for  the  unit  vectors 
parallel  to  the  revolute  axes. 


d  =  k,  62  =  j,  63  =  i 

while  vector  is  directed  from  the  origin  of  Ti  to  that  of  for  /  =  1,2,3. 
Hence, 


ai  =  —a\,  sl2  =  a(j  —  k),  SI3  =  a(i  k) 
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Likewise,  the  position  vectors  of  the  centers  of  mass,  for  i  =  I, 
with  respect  to  the  origins  of  their  respective  frames,  are  given  by 

Pi  =  ^«(-i+j) 

Pi  =  +  2j  -k) 

Pi  =  i«(2i  +  k) 

We  can  now  calculate  the  various  six-dimensional  arrays  t/y ,  for  i  = 
and  7=1  till  /,  i.e.. 


til  — 


ei 

k 

01  X 

_-(a/2)(i+j)_ 

t21  — 


t22  — 


ei 

k 

_ei  X  (ai  +  P2)_ 

_-(a/2)(2i+j)_ 

e2 

• 

J 

,02  X  P2_ 

_-(a/2)(i  +  k)_ 

t31  — 


ei 

■  k  ■ 

X  (ai  +  a2  +  P2,)_ 

_—ai_ 

t32  — 


62 

• 

J 

.62  X  (a2  +  Ps), 

_-(a/2)(i  +  2k)_ 

t33  — 


e3 

• 

1 

03  X  /)3_ 

_-(a/2)j_ 

and  so,  the  18x3  matrix  T  is  given  by 


k  0  0 

-(^/2)(i+j)  0  0 

k  j  0 

-(a/2)(2i-\-  i)  -(a/2)(i-\-k)  0 

k  j  i 


|_  —ai  —(a/2)(i-\-2k)—(a/2)} 
Moreover,  the  6x6  inertia  dyad  of  the  i  th  link  takes  the  form 


II  O 
O  ml 


i  =  1,2,3 


2,  and  3, 


1,  2,  3, 
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with  1  and  O  denoting  the  3x3  identity  and  zero  matrices,  respectively.  Thus, 
the  18  X  18  system  mass  matrix  is  given  as 

M  =  diag(Mi,  M2,  M3) 

and  the  3x3  generalized  inertia  matrix  I  of  the  manipulator  is 

I  =  T^MT 

whose  entries  are  given  by 

hi  =  tf^Mitii  +  t2iM2t2l  +  t^iM3t3i 
hi  =  t21^2t22  +  t^iM3t32  =  hi 
hi  =  t^iM3t33  =  hi 
hi  =  t22^2t22  +  t^2^3t32 
hi  =  t^2^3t33  =  hi 
hi  =  tr3M3t33 

Upon  expansion,  the  foregoing  expressions  yield 


1  2 

I  =  -ma 

A 

"11  4  0" 

"3  0  0" 

4  70 

+ 1 

020 

4 

_0  0  1_ 

_0  0  1_ 

(b)  Now,  the  time-rate  of  change  of  I,  I,  is  calculated  as 

i  =  T^MT  +  t^MT  +  T^(WM  -  MW)T 

We  proceed  first  to  compute  T.  This  time-derivative  is  nothing  but  the  18x3 
matrix  whose  entries  are  the  time-derivatives  of  the  entries  of  T,  namely,  ,  as 
given  in  Eq.  (7.84),  which  is  reproduced  below  for  quick  reference: 

ii=\ 

L("y  ^  Cy)  X  r,-y  +  ey  X  r,y_ 
where  is  given,  in  turn,  by 

Yij  =  Wy  X  ay  +  . . .  +  (Oi-I  X  a,_i  +  COi  X  p- 

Hence,  we  will  need  vectors  cu/ ,  for  f  =  1,  2,  and  3.  These  are  calculated  below: 


(Oi  =  Oiti  =  pk 
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(02  =  dlti  +  02^2  =  /?(j  +  k) 

(03  =  0iei  +  02^2  +  03^3  =  ;?(i  +  j  +  k) 


We  have,  therefore, 


61 

0 

0 

ei5  X  /Oj  +  Cl  X  p^_ 

Cl  X  (wi  X  Pi)_ 

—  p 

_(l/2)a(i-j)_ 

ei 

ei  X  (ai  +  P2)  +  ei  X  (ai  +  P2) 


0 

Cl  X  (wi  X  ai  +  «2  X  P2) 


0 

(l/2)aj 


62 

,62  X  P2  +  62  X  P2 


pei  X  €2 

(pei  X  62)  X  /)2  +  62  X  [/2(ei  +  62)  X  P2] 


—1 

-(l/2)a(i+j-k) 


Cl 

Cl  X  (ai  +  32  +  P3)  +  61  X  (ai  +  32  +  P3) 


0 

01  X  (wi  X  ai  +  0)2  X  ^2  +  W3  X  p^) 


0 

01  X  [p^i  X  ai  +  +  02)  X  a2  +  p{^\  +  02  +  63)  X  p^] 


02  X  (a2  +  p^)  +  02  X  (a2  +  p^)} 

pei  X  02 

(pei  X  02)  X  (a2  +  P2)  +  pe2  x  [(01  +  02)  x  (a2  +  p^)] 


=  P 

• 

—1 

_-(l/2)a(2i  +  j  - 

-k). 

• 

t33  = 

63 

_e3  X  yOj  +  63  X  P2_ 

— 

(O2  X  63 

_(«2  X  63)  X  /O3  +  63  X  (W3  X  P2)_ 
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^  pidi  +  62)  X  es 

_/?[(ei  +  62)  X  es]  X  /)3  +  /?e3  x  [(d  +  62  +  63)  x  /)3]_ 

j  ^  r  Pi^2  -  ei) 

lp{^2  -  ei)  X  /03  +  /7[(e3  •  /03)(ei  +  62  +  63)  -  /)3]. 

r  j-k 

^  (l/2)a(i  — k) 

Now,  let 

P  =  T^MT 

whose  entries  are  displayed  below: 

=  tuMitii  +  t2iM2t2l  +  t^iM3t3i 

Pu  =  ^21^2^22  +  t^iM3t32 

Pn  =  t^iM3t33 

Pll  =  t22M2t2l  +  t^2^3i31 

P22  =  t22M2t22  +  t^2^3i32 

P23  =  t^2^3t33 

7^31  =  t^3M3t3i 

P32  =  t^3M3t32 

P33  =  t^3M3t33 

Upon  performing  the  foregoing  operations,  we  end  up  with 

—{l/A)a^m  (l/4)a^m  —{\/2)a^m  —  I 

T^MT  = /?  — (l/2)a^m  0  (l/4)a^m  +  /  =P 

_{\/2)a^m  {l/4)a^m  —  I  0 

—T 

the  second  term  of  the  above  expression  for  I  simply  being  P  .  In  order  to 
compute  the  third  term,  we  need  the  products  WM  and  MW.  However,  it  is 
apparent  that  the  latter  is  the  negative  of  the  transpose  of  the  former,  and  so,  all 
we  need  is  one  of  the  two  terms.  Furthermore,  note  that  since  both  matrices  M 
and  W  are  block-diagonal,  their  product  is  block-diagonal  as  well,  namely, 

WM  =  diag(WiMi,  W2M2,  W3M3) 


where  for  i  =  1,2,  and  3, 
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with  O  denoting  the  3x3  zero  matrix,  while  ^  /  is  the  cross-product  matrix  of 
vector  (Oi .  Moreover, 


W,M, 


O 

o  o 


Therefore,  W/M,  is  skew- symmetric;  as  a  consequence,  WM  is  also  skew- 
symmetric,  and  the  difference  WM  —  MW  vanishes.  Hence,  in  this  particular 
case,  I  reduces  to 


i  =  p  + 


That  is. 


—(l/2)a^m  (5/4)a^m  —I 
{5/A)a^m  0  a^m  + 1 

—I  (l/2)a^m  0 


(c)  Now,  the  term  of  Coriolis  and  centrifugal  forces  can  be  computed  in  two  ways, 

namely,  (a)  as  (T^MT  +  T^WMT)^,  and  (b)  by  using  the  Newton-Euler 

•  • 

algorithm  with  Oi  =  0,  for  i  =  1,2,  and  3.  We  proceed  in  these  two  ways 
in  order  to  verify  the  correctness  of  our  results. 

In  proceeding  with  the  first  alternative,  we  already  have  the  first  term  in  the 
foregoing  parentheses;  the  second  term  is  now  computed.  First,  we  note  that 


WMT  = 


WiMitii  0  0 

W2M2t2l  W2M2t22  0 

W3M3t3i  W3M3t32  W3M3t33 


with  0  defined  as  the  six-dimensional  zero  vector.  The  foregoing  nontrivial  six¬ 
dimensional  arrays  are  computed  below: 


WiMitii  = 

W2M2t2l  = 


W2M2t22  — 


/fti  o' 

k 

mik 

o  o_ 

_-(a/2)(i+j)_ 

- 

0 

19.2  O' 

k 

- 

7s^2k 

o  o 

_-(a/2)(2i+j) 

- 

0 

192  O' 

• 

J 

o  o 

-{a/2){\  +  k)_ 

0 

0 

0 

pi 
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+  k)  xj 

0 


=  pi 


— 1 
0 


W3M3t3i  — 


7S^3  O 

o  o 


k 

—a\ 


W3M3t32  — 


• 

J 

-{a/2){\  +  2k)_ 

0 

W3M3t33  — 


pl(\  +  j  +  k)  X  k 

0 

7S^3  O 

o  o 

’pl{\  +  j  +  k)  X  j' 

0 

7S^3  O 

o  o 

pl(\  +  j  +  k)  X  i 

0 


7S^3k 

0 

=  pi 


i-J 

0 


=  pi 


-i  +  k 
0 


• 

1 

■/ftai' 

_-(a/2)j_ 

0 

=  pi 


j-k 

0 


where  0  now  denotes  the  three-dimensional  zero  vector.  Therefore, 


WMT  =  pi 


0  0  0 

0  0  0 

i  -i  0 

0  0  0 

i-j-i  +  kj-k 
0  0  0 


and  hence. 


T^WMT  =  pi 


0  1  -1 
-1  0  1 
1  -1  0 


which  turns  out  to  be  skew- symmetric.  Notice,  however,  that  this  will  not 
always  be  the  case.  The  reason  why  the  above  product  turned  out  to  be  skew- 
symmetric  in  this  example  is  that  the  individual  matrices  W/  and  M,  commute, 
a  consequence  of  the  assumed  inertial  isotropy,  which  leads  to  the  isotropy  of 
matrices  7  ,  for  i  =  1,2,  and  3.  Now,  we  have 

T^MT  +  T^WMT  =  jpA 


with  A  defined  as 


328 


7  Dynamics  of  Serial  Robotic  Manipulators 


— (3/4)(2^m  {l/A)a^m-\-I  —{\/2)a^m —  21 

—{\/2)a^m  —  I  0  {1/ A)a^m -\- 21 

{2/A)a^m-\-I  {\/A)a^m  —  2I  0 


Hence,  the  term  of  Coriolis  and  centrifugal  forces  is 


(T^Mt  +  T^WMT)I9  = 


{\/2)a^m  —  I 
—{\/A)a^m  +  I 
a^m  —  I 


thereby  completing  the  desired  calculations. 

Now,  in  order  to  verify  the  correctness  of  the  above  results,  we  will  compute 

•  • 

the  same  term  using  the  Newton-Euler  algorithm.  To  this  end,  we  set  Of  =  0,  for 
i  =  1,2,  and  3,  in  that  algorithm,  and  calculate  the  desired  expression  as  the  torque 
required  to  produce  the  joint  rates  given  above. 

Since  we  have  already  calculated  the  angular  velocities,  we  will  skip  these  calcu¬ 
lations  here  and  limit  ourselves  to  center-of-mass  velocities,  angular  accelerations, 
and  center-of-mass  accelerations.  We  thus  have 


Cl 


C2 


C3 


0)1  X  =  pkx 


(i-j) 


--api\+j) 


Cl  +  <wi  X  (ai  -  Pi)  +  «2  X  /02 

-  kx  (i  +  j)  +  (j  +  k)  x  (i  +  j  -  k)]  =  -^ap(3i  +  j  +  k) 
62  +  W2  ^  (^2  —  P2)  "I"  ^3  ^  /®3 

~ap[3i  +  j  +  k  +  (j  +  k)  X  (i  +  k)  -  (i  +  j  +  k)  X  (2i  +  k)] 
-l«/’(3i+j  +  2k) 


Now,  the  acceleration  calculations  are  implemented  recursively,  which  yields 


0)1  =  Oiti  =  0 

ft>2  =  <w  1  +  <w  1  X  02^2  =  p  kxj  =  -p  i 

«3  =  <W2  +  <W2  X  ^363  =  -pH  +  p^(j  +  k)  X  i  =  -p^ii  -  j  +  k) 


kx-(-i+j) 


Cl  =  X  /0[  +  <y  1  X  (fe>i  X  /)[)  =  ap  k  x 
C2  =  Cl  +  «i  X  (ai  -  />i)  +  <Wi  X  [«!  X  (ai  -  />i)]  +  <W2  x  /)2 


1 


+  CO2X  ((02X  P2)  =  ^ap^d  -  j)  +  0  +  ^ap^d  +  j) 
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+  2k)  +  ^ap^{-2\  -  3j  +  3k)  =  ^-ap'^{-Ai  +  k) 

C3  =  C2  +  W2  X  (a2  -  Pi)  +  «2  X  [(O2  X  (&!  -  Pi)]  +  0)3  X  /Oj 

=  +  <W3  X  (W3  X  /Oj)  =  ^-ap'^{-Ai  +  k)  -  ^ap'^i  +  ^-ap'^{2\  -  j  +  k) 

+  -  j  -  2k)  +  la/?^(-3i  +  3j)  =  -2ap'^i 

With  the  foregoing  values,  we  can  now  implement  the  inward  Newton-Euler 
recursions,  namely. 


ff  =  m3C3  —  f  =  —m(2ap^})  —  0  =  —lamp^j 
nf  =  13(03  +  0)3  X  13(03  -  n  +  /)3  X  ff 

=  —Ip^ii  — j  +  k)  +  0  —  0  —  a^mp^{—\  +  2k) 
=  —Ip^ii  —  j  +  k)  +  a^mp^ii  —  2k) 


D  r)  1  O  X 

=  m2C2  +  f3  =  -amp  {—A}  +  k)  —  amp  ]  =  -amp  (-6j  +  k) 


1 

2 


—  I2(02  +  0)2  X  l20)2  +  +  (a2  —  P2)  ^  P2  ^  ^2 

=  —p^Ii  +  0  —  Ip^(i  —  j  +  k)  H — a^mp^ii  —  2k)  +  a^mpH 


+  -a^mp^(—4i  —  j  —  6k)  =  —Ip^{2\  —  j  +  k)  +  -a^mp^{2\  —  j  —  10k) 

ff  =  WiCi  +  ff  =  -amp  “  j)  +  -amp  2(-6j  +  k) 

1 

=  -amp  '(i-7j  +  k) 

nf  =  IiO)i  +  0)1  X  IiO)i  +  nf  +  (ai  -  p^)  x  ff  +  x  ff 
=  0  +  0  —  p^I(2i  —  j  +  k)  +  -a^mp^  (2i  —  j  —  10k) 

—  -a^mp^(i  —  j  —  6k)  +  -a^mp^(i  +  j  —  6k) 

=  -Ip^{2\  -  j  +  k)  +  ^a^mp^{2\^- }  +  2k) 


and  hence. 


r3  =  nf  •  03  =  —Ip^  +  a^mp^ 
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r2  =  nf 
ri  =  nf 


02  =  Ip^  —  -a^mp^ 
01  =  —Ip^  H — a^mp 


2 


thereby  completing  the  calculation  of  the  term  containing  Coriolis  and  centrifugal 
forces,  i.e., 


C(0j)0 


—Ip^  +  {ll2)a^mp^ 
Ip^  —  {\/ A)a^mp^ 
—Ip^  +  a^mp^ 


As  the  reader  can  verify,  the  natural  orthogonal  complement  and  the  Newton-Euler 
algorithm  produce  the  same  result.  In  the  process,  the  reader  may  have  realized  that 
when  performing  calculations  by  hand,  the  Newton-Euler  algorithm  is  more  prone 
to  errors  than  the  natural  orthogonal  complement,  which  is  more  systematic,  for  it 
is  based  on  matrix-times-vector  multiplications. 


7. 6. 1  Planar  Manipulators 

The  application  of  the  natural  orthogonal  complement  to  planar  manipulators  is 
straightforward.  Here,  we  assume  that  the  manipulator  at  hand  is  composed  of 
n  links  coupled  by  n  joints  of  the  revolute  or  the  prismatic  type.  Moreover, 
for  conciseness,  we  assume  that  the  first  link,  labeled  the  base,  is  fixed  to  an 
inertial  frame.  We  now  adopt  the  planar  representation  of  the  twists  and  wrenches 
introduced  in  Sect.  5.7;  that  is,  we  define  the  twist  of  the  /th  link  and  the  wrench 
acting  on  it  as  three-dimensional  arrays,  namely, 

— 

l^i 

where  coi  is  the  scalar  angular  velocity  of  this  link;  C/  is  the  two-dimensional  velocity 
of  its  center  of  mass,  Q  \  nt  is  the  scalar  moment  acting  on  the  link;  and  f/  is  the 
two-dimensional  force  acting  at  Q .  Moreover,  the  inertia  dyad  is  now  a  3  x  3  matrix, 
i.e.. 


ni 

I 


(7.107) 


(7.108) 


with  li  defined  as  the  scalar  moment  of  inertia  of  the  i  th  link  about  an  axis  passing 
through  its  center  of  mass,  in  the  direction  normal  to  the  plane  of  motion,  while  0  is 
the  two-dimensional  zero  vector  and  1  is  the  2x2  identity  matrix. 
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Furthermore,  the  Newton-Euler  equations  of  the  i  th  link  take  on  the  forms 

Hi  =  licbi  (7.109a) 

f.  =  rriiCi  (7.109b) 

and  so,  these  equations  can  now  be  cast  in  the  form 

+  wf,  i  =  I, ...  ,n  (7.110) 

where  we  have  decomposed  the  total  wrench  acting  on  the  i  th  link  into  its  working 
component  supplied  by  the  environment  and  accounting  for  motor  and  joint 
dissipative  torques,  and  wf ,  the  nonworking  constraint  wrench,  supplied  by  the 
neighboring  links  via  the  coupling  joints.  The  latter,  it  is  recalled,  develop  no  power, 
their  sole  role  being  to  keep  the  links  together.  An  essential  difference  from  the 
general  six-dimensional  counterpart  of  the  foregoing  equation,  namely,  Eq.  (7.48), 
is  the  lack  of  a  quadratic  term  in  cof  in  Eq.  (7.109a)  and  consequently,  the  lack  of  a 
W;  Mi  ii  term  in  Eq.  (7.110). 

Upon  assembling  the  foregoing  3n  equations  of  motion,  we  obtain  a  system  of 
3n  uncoupled  equations  in  the  form 


Mt  = 

Now,  the  wrench  accounts  for  active  forces  and  moments  exerted  on  the 
manipulator,  and  so  we  can  decompose  this  wrench  into  an  actuator- supplied  wrench 
and  a  gravity  wrench  . 

In  the  next  step  of  the  formulation,  we  set  up  the  kinematic  constraints  in  linear 
homogeneous  form,  as  in  Eq.  (7.50),  with  the  difference  that  now,  in  the  presence 
of  n  kinematic  pairs  of  the  revolute  or  the  prismatic  type,  K  is  a  3/i  x  3/7  matrix. 
Moreover,  we  set  up  the  twist-shape  relations  in  the  form  of  Eq.  (7.56),  except 
that  now,  T  is  a  3/7  x  /7  matrix.  The  derivation  of  the  Euler-Lagrange  equations 
for  planar  motion  using  the  natural  orthogonal  complement,  then,  parallels  that  of 
general  three-dimensional  motion,  the  model  sought  taking  the  form 


1(0)0  +  C(0,0)0  =  T  +  y  +  5  (7.111a) 

with  the  definitions 

I(^)  =  T^MT,  C(^,^)  =  T^MT,  (7.111b) 

y  =  T^w®,  5=T^w^  (7.111c) 


We  can  illustrate  best  this  formulation  with  the  aid  of  the  example  below. 
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Example  7.6.2  (Dynamics  of  a  Planar  Three -Revolute  Robot).  Derive  the  model  of 
the  robot  of  Fig.  7.1,  under  the  assumptions  of  Example  7.3.1,  but  now  using  the 
natural  orthogonal  complement. 

Solution:  We  start  by  deriving  all  kinematics-related  variables,  and  thus, 

(i>\  =  0\,  0)2  =  0\  -\-  O2,  C02  =  di  O2  O2 


Furthermore, 

ti  = 

t2  =  ^lt21  +  ^2t22 
h  =  +  ^2t32  +  ^3t33 


where 


til 

t21 

t22 

t31 

t32 

t33 


1 

1 

1 

Esu_ 

_(l/2)Eai_ 

1 

1 

1 

ES21_ 

_E(ai  +  P2)_ 

_E(ai  +  (l/2)a2). 

1 

1 

1 

ES22_ 

.E/>2. 

_(l/2)Ea2. 

1 

ES3i_ 

— 

1 

_E(ai  +  a2  + 

— 

1 

_E(ai  +  a2  +  (l/2)a3) 

1 

ES32_ 

— 

1 

_E(a2  +  pf)_ 

— 

E(a2 

+ 

1 

1 

E/>3. 

.(l/2)Ea3. 

and  hence,  the  9x3  twist-shaping  matrix  T  becomes 


1  0  0 

(l/2)Eai  0  0 

1  1  0 

E(ai  +  (l/2)a2)  (l/2)Ea2  0 

1  1  1 


E(ai  +  a2  +  (l/2)a3)  E(a2  +  (l/2)a3)  (l/2)Ea3 


The  9  X  9  matrix  of  inertia  dyads  of  this  manipulator  now  takes  the  form 


M  =  diag(Mi,  M2,  M3) 
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with  each  3  x  3  M/  matrix  defined  as 


■/,  0^' 
0  mil 


Now,  the  3x3  generalized  inertia  matrix  is  readily  derived  as 

I  =  T^MT 


whose  entries  are  given  below: 

^11  “  tuMitii  +  t2iM2t2l  +  t^iM3t3i 

hi  =  t2iM2t22  +  t^iM3t32  =  hi 

hs  =  triM3t33  =  hi 

hi  =  ^21^2hl  +  t^2^3t32 

hi  =  t^2^3t33  =  hi 

hi  =  t^3M3t33 


Upon  expansion,  the  above  entries  result  in  exactly  the  same  expressions  as  those 
derived  in  Example  7.3.1,  thereby  confirming  the  correctness  of  the  two  derivations. 
Furthermore,  the  next  term  in  the  Euler-Lagrange  equations  is  derived  below.  Here, 
we  will  need  T,  which  is  readily  derived  from  the  above  expression  for  T.  In  deriving 
this  time-derivative,  we  note  that  in  general,  for  /  =  1,2,3, 

a,- =  ftj,- Ea,- ,  E^a,- = -a; 


and  hence. 


t 


0 

0 

0 

(l/2)0iai 

0 

0 

0 

0 

0 

^lai  +  (l/2)^i2a2 

(l/2)^i2a2 

0 

0 

0 

0 

+  0i2a2  +  (l/2)0i23a3  ^I2a2  +  (l/2)^i23a3  (l/2)^i23a3 

•  •  •  •  •  •  • 

where  Ou  and  ^123  stand  for  0i  +  O2  and  0i  O2  O3,  respectively. 

We  now  can  perform  the  product  T^MT,  whose  (/,  j)  entry  will  be  represented 
as  fjiij .  Below  we  display  the  expressions  for  these  entries: 

1  •  m3 

/Xii  =  --[m2a\_a2S2  +  m2,{2a\_a2S2  +  a\_a2,S22)]02  -  — («ia3-523  +  a2a3‘S'3)03 
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1  ... 

Ml3  =  -:rm3(aia3S23  +  «2«3'S'3)(pi  +B2  +  63) 

1 

M21  =  -[m2aia2S2  +  m3(2aia2S2  +  aia3S23)]0i 

1  ... 

/^23  =  --mCllCl?>S2>{0i  +  02  +  03) 

1 

/^31  =  -m[{aia^S2?,  +<22<23^3)01  +  <22<23^302] 


1  . 

-m3(22<23^303 


/^32 

/^33 


1 

-m3a2a3S3(di  +  02) 


=  0 


Now,  let  us  define 


V  =  T^MTO 


whose  three  components  are  given  below: 


Vi  =  -[m2aia2S2  +  m3(2aia2S2  +  <3!i(23^23)]0102  -m3(aia3S23  +  <22<23^3)0103 

1 

2 


--[m2aia2S2  +  m3(2aia2S2  +  <2i(23^23)]02 


1 


i2 


-m3{aia3S23  +  <22<23^3)0203  “  “^3 ((2i(23^23  +  a2a3S3)0^ 


1 


V2  = 


1 


i2 


-m3(22<23^30203  “  -m3(22<23^303 

2 


V3  = 


1  .3  .  .  1 

-m3(aia3S23  +  <22(23^3)01  +  m3<22<23^30102  +  -m3<22<23^303 


i2 


The  mathematical  model  sought,  thus,  takes  the  form 


1(0)0 +V(0,0)  =  T  +  )/ 


where  5=0  because  we  have  not  included  dissipation.  Moreover,  y  is  derived  as 
described  below:  Let  wp  be  the  gravity  wrench  acting  on  the  ith  link,  then  being 
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W2« 

W3« 


and 


0 

G 

0 

G 

0 

,  W2  = 

,  W3  = 

- 1 

m 

1 

_ 1 

Therefore, 


y 


T^w 


G 


i.e.. 


y 


miafEj  +  m2(2ai  +  a2)^Ej  +  m3[2(ai  +  a2)  +  a3)^Ej 

m2a[Ej  +  m3(2a2  +  a3)^Ej 
m3a[Ej 


But 


Hence, 


ry-r  rj-i 

a|  Ej  =  — a|  i  =  —<21  cos  0i 
a^Ej  =  -a^i  =  -<22cos(0i  +  O2) 
a^Ej  =  — =  —G3  cos(0i  +  ^2  +  O3) 


y 


-miaici  -  2m2(aici  +  <22Ci2)  -  2m3(aiCi  +  a2Ci2  +  <23Ci23) 

-m2<22Ci2  -  2m3(a2Ci2  +  <23Ci23) 

— m3a3Ci23 


with  the  definitions  for  Ci,  C12,  and  C123  introduced  in  Example  7.3.1.  As  the  reader 
can  verify,  the  foregoing  model  is  identical  to  the  model  derived  with  the  Euler- 
Lagrange  equations  in  that  example. 


7.6.2  Algorithm  Complexity 

The  complexity  of  this  algorithm  is  analyzed  with  regard  to  the  three  items  involved, 
namely,  (a)  the  evaluation  of  L,  (b)  the  solution  of  systems  (7.105a  and  b),  and  (c) 
the  computation  of  t  . 
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The  evaluation  of  L  involves,  in  turn,  the  three  following  steps:  (a)  the  computa¬ 
tion  of  P;  (b)  the  computation  of  I;  and  (c)  the  Cholesky  decomposition  of  I  into  the 
product  L^L. 


Algorithm  7.6.1: 

For  j=lto  n  step  1  do 


jj 


[  P j  ]j -ft 


'77 


Nyey 

Pj^j  xryyj 


;+i 


For  i  =  j+lto  n  step  1  do 


^7  ^  Q-f[^j]i 


if  R  then 


ij 


Qf  ]i  +  [Pi  ];  +  l 


N.e, 

ili^j  X  r,yj 


?  +1 


else 


0 

Ui^j 


? + 1 


endif 

enddo 

enddo 


(i.a)  In  the  computation  of  P,  it  is  recalled  that  H/ ,  ,  and  ,  and  consequently, 

Si  =  SLi  —  Pi,  are  constant  in  /7+1,  which  is  the  frame  fixed  to  the  ith  link. 
Moreover,  at  each  step  of  the  algorithm,  both  revolute  and  prismatic  pairs  are 
considered.  If  the  7  th  joint  is  a  revolute,  then  the  logical  variable  R  is  true; 
if  this  joint  is  prismatic,  then  R  is  false.  Additionally,  it  is  recalled  that  e^+i, 
in  Ti  -coordinates,  is  simply  the  last  column  of  Q/ .  The  columnwise  evaluation 
of  P,  with  each  array  in  Jy+i -coordinates,  is  described  in  Algorithm  7.6.1. 

Note  that  in  this  algorithm,  r/y  is  calculated  recursively  from  r/_i  y .  To  do  this, 
we  use  the  relation  between  these  two  vectors,  as  displayed  in  Fig.  7.8. 

(i.b)  Now  we  go  on  to  the  computation  of  I,  as  described  in  Algorithm  7.6.2.  In 
that  algorithm,  the  subscripted  brackets  indicate  that  the  vectors  inside  these 
brackets  are  represented  in  coordinates. 

(i.c)  Because  the  Cholesky  decomposition  of  a  positive-definite  matrix  is  a  standard 
item,  it  is  not  discussed  here.  This  step  completes  the  computation  of  L. 

(ii)  The  solution  of  systems  (7.105a  and  7.105b)  is  a  standard  issue  as  well,  and 
hence,  needs  no  further  discussion. 
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Fig.  7.8  Recursive  calculation  of  vectors  r/y 

(iii)  The  term  t  is  computed  using  the  recursive  Newton-Euler  formulation,  as 

•  • 

discussed  in  Sect.  7.4.  To  do  this,  we  calculate  t  by  setting  ^  =  0  in  that 
procedure,  which  introduces  a  slight  simplification  of  the  complexity  of  the 
inverse-dynamics  algorithm. 


Algorithm  7.6.2: 

For  j=lto  n  step  1 

do 

^Jj 

^  'Hk= j  [  ply 

For  i  =  j  +1  to 

n  step  1  do 

lij 

enddo 

enddo 

Below  we  determine  the  computational  complexity  of  each  of  the  foregoing 
steps. 

(i.a)  This  step  includes  Algorithm  7.6.1,  which  involves  two  nested  do-loops.  The 
first  statement  of  the  outermost  loop  involves  no  floating-point  operations; 
the  second  statement  involves  (a)  one  multiplication  of  a  matrix  by  a  vector, 
(b)  one  cross  product,  and  (c)  one  multiplication  of  a  scalar,  nj{= 
by  a  vector,  [ey]y+i,  which,  according  to  Eq.  (4.15),  equals  Oy,  the  third 
row  of  Qy .  In  light  of  Eq.  (4.7),  this  vector  involves  only  two  nonzero 
components,  the  product  /ty  [ey  ]y+i  thus  consuming  only  two  multiplications 
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and  zero  additions.  Of  the  last  three  items,  (a)  is  done  off-line,  for  the  matrix 
and  the  vector  factors  are  both  constant  in  -coordinates,  and  so,  this 
operation  is  not  counted.  Moreover,  item  (b)  is  nothing  but  the  cross  product 
of  vector  [ey]y+i  =  [0,  0,  1]^  by  vector  Yjj.  A  similar  operation  was 
already  discussed  in  connection  with  Algorithm  4.1  and  was  found  to  involve 
zero  floating-point  operations,  for  the  result  is,  simply,  [e^  x  ]y+i  = 
[—y,  X,  0]^,  with  X  and  y  denoting  the  Xj-^i  and  Fy+i  components  of  rjj. 
Hence,  item  (b)  requires  no  floating-point  operations,  while  item  (c)  requires 
2n  multiplications  and  zero  additions. 

The  innermost  do-loop,  as  pertaining  to  revolute  manipulators,  involves  two 
coordinate  transformations  between  two  consecutive  coordinate  frames,  from 
Jy-  to  Jy+i -coordinates,  plus  one  vector  sum,  which  consumes  \6in  —  i) 
multiplications  and  \  \{n  —  i)  additions;  this  loop  also  consumes  one  matrix- 
times-vector  multiplication,  one  cross  product  and  one  scalar-times-vector 
multiplication,  which  requires  lS(n  —  i)  multiplications  and  9(n  —  i)  additions. 
Thus,  the  total  numbers  of  operations  required  by  this  step,  for  an  -revolute 
manipulator,  are  Mia  multiplications  and  At  a  additions,  as  given  below: 


n 

Mia  =2n  +  ^34(«  -  i)  =  \ln^  -  \5n  (7.112a) 

i  =  l 
n 

Aia  =  Y^20{n  -  i)  =  10«2  -  lOn  (7.112b) 

i  =  l 


the  presence  of  prismatic  pairs  reducing  the  above  figures. 

(i.b)  This  step,  summarized  in  Algorithm  7.6.2,  is  also  composed  of  two  do-loops, 
each  containing  the  inner  product  of  two  six-dimensional  arrays,  and  hence, 
requires  six  multiplications  and  five  additions.  Moreover,  in  the  outermost 
do-loop,  this  operation  is  performed  n  times,  whereas  in  the  innermost  loop, 
YTi=i^^  ~  0  times,  i.e.,  n{n  —  l)/2  times.  Thus,  the  step  requires  Muy 
multiplications  and  Any  additions,  as  given  below: 


Mil)  =  3/1^  +  ?>n,  Ail)  =  H — ^  (7.113) 

(i.c)  This  step  performs  the  Cholesky  decomposition  of  a  /7  x  symmetric  and 
positive-definite  matrix,  a  standard  operation  that  requires  multiplications 
and  Aic  additions  (Dahlquist  and  Bjorck  1974),  namely. 


1,1,1  1,1,1 

Mic  =  -n  +  -n  +  -n,  Aj^  =  -n  +  -n  +  -n 


(7.114) 


(ii)  In  this  step,  the  two  triangular  systems  of  equations,  Eqs.  (7.105a  and  b), 

•  • 

are  solved  first  for  x  and  then  for  0 .  The  numbers  of  operations  it  takes  to 
solve  each  of  the  two  systems,  as  derived  by  Dahlquist  and  Bjorck  (1974), 
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are  repeated  below  for  quick  reference;  these  are  labelled  Mu  and  An, 
respectively,  i.e., 


Mii=n^,  Aii=n^-n  (7.115) 

_  ^  •• 

(iii)  In  this  step,  r  is  computed  from  inverse  dynamics,  with  w  =  0  and  ^  =  0. 

If  this  calculation  is  done  with  the  Newton-Euler  formulation,  we  then  have 

the  computational  costs  given  in  Eq.  (7.43),  and  reproduced  below  for  quick 

reference: 


Mm  =  Uln-11,  Am  =  nOn  -  14  (7.116) 

Because  of  the  simplifications  introduced  by  setting  the  joint  accelerations  equal 
to  zero,  the  foregoing  figures  are,  in  fact,  slightly  lower  than  those  required  by  the 
general  recursive  Newton-Euler  algorithm. 

Thus,  the  total  numbers  of  multiplications  and  additions  required  for  the  forward 
dynamics  of  a  w -revolute,  serial  manipulator  are 

1  ,  37  ,  367  1  ,  23  ,  298 

Mf  =  -n^  H - H - n  —22,  Af  =  -n^  H - H - n  —  14  (7.117) 

^  6  2  3  ^62  3 

In  particular,  for  a  six-revolute  manipulator,  one  obtains 

M/  =  1,450,  .4/ =  1,068  (7.118) 

Upon  introducing  a  modified  Denavit-Hartenberg  labeling  of  coordinate  frames  and 
a  very  careful  management  of  the  computations  involved  the  number  of  floating¬ 
point  operations  became  1,353  multiplications  and  1,165  additions  (Angeles  and 
Ma  1988).  Nevertheless,  the  total  number  of  operations,  2,518,  remained  the  same. 


7.6.3  Simulation 

The  purpose  of  the  algorithm  introduced  above  is  to  enable  us  to  predict  the  behavior 
of  a  given  manipulator  under  given  initial  conditions,  applied  torques,  and  applied 
loads.  The  ability  of  predicting  this  behavior  is  important  for  several  reasons:  for 
example,  in  design,  we  want  to  know  whether  with  a  given  selection  of  motors, 
the  manipulator  will  be  able  to  perform  a  certain  typical  task  in  a  given  time 
frame;  in  devising  feedback  control  schemes,  where  stability  is  a  major  concern, 
the  control  engineer  cannot  risk  a  valuable  piece  of  equipment  by  exposing  it  to 
untested  control  strategies.  Hence,  a  facility  capable  of  predicting  the  behavior  of  a 
robotic  manipulator,  or  of  a  system  at  large,  for  that  matter,  becomes  imperative. 

The  procedure  whereby  the  motion  of  the  manipulator  is  determined  from  initial 
conditions  and  applied  torques  and  loads  is  known  as  simulation.  Since  we  start 
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with  a  second-order  /7 -dimensional  system  of  ODE  in  the  joint  variables  of  the 
manipulator,  we  have  to  integrate  this  system  in  order  to  determine  the  time-histories 
of  all  joint  variables,  which  are  grouped  in  vector  0 .  With  current  software  available, 
this  task  has  become  routine  work,  the  user  being  freed  from  the  quite  demanding 
task  of  writing  code  for  integrating  systems  of  ODE.  Below  we  discuss  a  few  issues 
pertaining  to  the  implementation  of  the  simulation-related  algorithms  available  in 
commercial  software  packages. 

As  a  rule,  simulation  code  requires  that  the  user  supply  a  state- variable  model  of 
the  form  of  Eq.  (7.45),  with  the  state- variable  vector,  or  state- vector  for  brevity,  x, 
and  the  input  or  control  vector  u  defined  as 


(7.119) 


With  the  above  definitions,  then  we  can  write  the  state- variable  equations,  or  state 
equations  for  brevity,  in  the  form  of  Eq.  (7.45),  with  f(x,  t)  given  by 


f(x,  t) 


-i{e)-^[C{e,ilf)ilf  -  8(0,  f)  -  y(0)]  +  i(^)-'t(0 


(7.120) 


thereby  obtaining  a  system  of  2n  first-order  ODE  in  the  state-variable  vector  x 
defined  above.  Various  methods  are  available  to  solve  the  ensuing  initial- value 
problem,  all  of  them  being  based  on  a  discretization  of  the  time  variable.  That  is,  if 
the  behavior  of  the  system  is  desired  in  the  interval  to  <  t  <  tf,  then  the  software 
implementing  these  methods  provides  approximations  {  to  the  state- variable 

vector  at  a  discrete  set  of  instants,  {  4  with  t^  =  tf. 

The  variety  of  methods  available  to  solve  the  underlying  initial-value  problem 
can  be  classified  into  two  main  categories,  explicit  methods  and  implicit  methods. 
The  former  provide  y^+i  explicitly  in  terms  of  previously  computed  values.  On  the 
contrary,  implicit  methods  provide  y^+i  in  terms  of  previously  computed  values  y^, 
y^_i,  . . .,  etc.,  and  y^+i  itself.  Eor  example,  in  the  simplest  of  implicit  methods, 
namely,  the  backward  Euler  method,  we  can  approximate  the  integral  of  f  in  the 
interval  4  <  t  <  4+1  by  resorting  to  the  trapezoidal  rule  (Kahaner  et  al.  1989), 
which  leads  to  the  expression 


y<:+i  =  y*  +  hki{tk+i,yk+i) 


(7.121) 


In  Eq.  (7.121),  hk  is  the  current  time-step  4+i  —  4  and  f(4+i,y^+i)  can  be  an 
arbitrary  function  of  y^+i.  If  this  function  is  nonlinear  in  the  said  variable,  then, 
a  direct — as  opposed  to  iterative — computation  of  y^+i  is  very  unlikely.  Hence, 
most  likely  an  iterative  scheme  must  be  implemented  at  every  integration  stage  of 
an  implicit  method.  While  this  feature  might  render  implicit  schemes  unattractive, 
they  offer  interesting  advantages.  Indeed,  the  iterative  procedure  mentioned  above 
requires  a  tolerance  to  decide  when  and  whether  the  procedure  has  converged.  The 
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convergence  criterion  imposed  thus  brings  about  a  self-correcting  effect  that  helps 
keep  the  unavoidable  truncation  error  under  control.  This  error  is  incurred  when 
approximating  both  the  time  derivative  x  and  the  integral  of  f  by  floating-point 
operations. 

Current  software  provides  routines  for  both  implicit  and  explicit  methods,  the 
user  having  to  decide  which  method  to  invoke.  Of  the  explicit  methods  in  use, 
by  far  the  most  common  ones  are  the  Runge-Kutta  methods.  Of  these,  there  are 
several  versions,  depending  on  the  number  of  evaluations  of  the  function  f(^/,y;), 
for  various  values  of  /,  that  they  require.  A  two-stage  Runge-Kutta  method,  for 
example,  requires  two  function  evaluations,  while  a  four-stage  Runge-Kutta  method 
requires  four.  The  self-correcting  feature  of  implicit  methods,  not  present  in  Runge- 
Kutta  methods — to  be  sure,  implicit  Runge-Kutta  methods  also  exist  (Gear  1971), 
but  these  are  less  common  than  their  explicit  counterparts — is  compensated  for  by  a 
clever  strategy  that  consists  in  computing  y^+i  using  two  Runge-Kutta  schemes  of 
different  numbers  of  stages.  What  is  at  stake  here  is  the  magnitude  of  the  local  error 
in  computing  y^+i,  under  the  assumption  that  y^  is  error- free.  Here,  the  magnitude 
of  the  error  is  of  order  h^ ,  where  p  is  the  order  of  the  method  in  use.  In  Runge-Kutta 
methods,  the  order  of  the  method  is  identical  to  its  number  of  stages.  In  general,  a 
method  is  said  to  be  of  order  p  if  it  is  capable  of  computing  exactly  the  integral  of 
an  ordinary  differential  equation,  provided  that  the  solution  is  known  to  be  a  pi\\- 
degree  polynomial.  Now,  upon  computing  y^+i  using  two  Runge-Kutta  schemes 
with  N  and  A  +  1  stages,  we  can  compare  the  two  computed  values  reported  by 
each  method,  namely,  y^^^  and  y^^/ .  If  a  norm  of  the  difference  of  these  two  values 
is  smaller  than  a  user-prescribed  tolerance,  then  the  step  size  in  use  is  acceptable.  If 
not,  then  the  step  size  is  halved,  and  the  process  is  repeated  until  the  foregoing  norm 
is  within  the  said  tolerance.  The  most  common  Runge-Kutta  methods  are  those 
combining  two  and  three  stages  and  those  combining  four  and  five. 

A  drawback  of  Runge-Kutta  methods  is  their  inability  to  deal  with  what  are 
known  as  stiff  systems,  first  identified  by  Gear  (1971).  As  defined  by  Shampine 
and  Gear  (1979),  a  system  of  ordinary  differential  equations  is  said  to  be  stiff  if 
it  is  not  unstable  and  its  linear  part — i.e.,  the  linear  part  of  the  series  expansion 
of  f,  evaluated  at  the  current  instant — comprises  a  coefficient  matrix  that  has  an 
eigenvalue  with  a  negative  real  part  whose  absolute  value  is  much  greater  than  that 
of  the  other  eigenvalues.  In  other  words,  stiff  systems  of  ODE  are  stable  systems 
with  very  different  time  scales.  Thus,  stiff  systems  are  not  inherently  difficult  to 
integrate,  but  they  require  a  special  treatment.  Gear’s  method,  which  is  implicit, 
provides  exactly  the  means  to  handle  stiff  systems.  However,  methods  like  Runge- 
Kutta’s,  with  excellent  performance  for  nonstiff  systems,  perform  rather  poorly  for 
stiff  systems,  and  the  other  way  around.  The  mathematical  models  that  arise  in 
robotic  mechanical  systems  are  likely  to  be  stiff  because  of  the  various  orders  of 
magnitude  of  the  physical  parameters  involved.  For  example,  robotic  manipulators 
are  provided,  usually,  with  links  close  to  the  base  that  are  heavy  and  with  links  far 
from  the  base  that  are  light.  As  a  consequence,  when  simulating  robotic  mechanical 
systems,  a  provision  must  be  made  for  numerical  stiffness. 
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Commercial  software  for  scientific  computations  offers  Runge-Kutta  methods 
of  various  orders,  with  combinations  thereof.  For  example,  IMSL  offers  excellent 
FORTRAN  routines,  like  IVPRK,  for  the  implementation  of  Runge-Kutta  methods, 
while  Matlab’s  Simulink  toolbox  offers  the  C  functions  ode2  3  and  ode4  5  for  the 
implementation  of  second-and-third  and  fourth-and-fifth-order  Runge-Kutta  meth¬ 
ods.  With  regard  to  stiff  systems,  IMSL  offers  a  subroutine,  IVPAG,  implementing 
both  Adams’s  and  Gear’s  methods,  while  Simulink  offers  the  adams  and  gear 
functions  for  the  implementation  of  either  of  these.  Since  Matlab  is  written  in  C, 
communication  between  Matlab  and  FORTRAN  programs  is  not  as  direct  as  when 
using  IMSL,  which  may  be  disappointing  to  FORTRAN  users.  Details  on  linking 
FORTRAN  code  with  Matlab  and  other  related  issues  are  discussed  in  the  pertinent 
literature  (Etter  1997).  Moreover,  the  FORTRAN  SDRIV2  subroutine  (Kahaner 
et  al.  1989)  comprises  features  that  allow  it  to  handle  both  stiff  and  nonstiff  systems. 


7.7  Incorporation  of  Gravity  into  the  Dynamics  Equations 


Manipulators  subjected  to  gravity  fields  have  been  discussed  in  Sect.  7.4  in  connec¬ 
tion  with  the  Newton-Euler  algorithm  and  with  Kane’s  equations.  As  found  in  that 
section,  gravitational  forces  can  be  incorporated  into  the  underlying  models  without 
introducing  any  major  modifications  that  would  increase  the  computational  load 
if  the  method  of  Luh  et  al.  (1980)  is  adopted.  Within  this  approach,  gravitational 
forces  are  taken  into  account  by  defining  the  acceleration  of  the  center  of  mass  of 
the  0th  link,  the  base  link,  as  equal  to  — g,  the  negative  of  the  gravity-acceleration 
vector.  The  effect  of  this  approach  is  to  propagate  the  gravity  effect  into  all  the  links 
composing  the  manipulator.  Thus,  the  kinematics  algorithm  of  Sect.  7.4  need  not  be 
modified  in  order  to  include  gravity  forces,  for  all  that  is  needed  is  to  declare 

[co]i  ^  [-g]i  (7.122) 


If  inverse  dynamics  is  computed  with  the  natural  orthogonal  complement,  then  the 
twist-rate  of  the  first  link  will  have  to  be  modified  by  adding  a  nonhomogeneous 
term  to  it,  thereby  accounting  for  the  gravity- acceleration  terms.  That  is. 


+ 


(7.123) 


Otherwise,  the  foregoing  algorithms  require  no  modifications.  Furthermore,  with 
regard  to  simulation,  it  is  pointed  out  that  the  t  term  defined  in  Eq.  (7.104),  and 
appearing  in  the  right-hand  side  of  Eq.  (7.105a),  is  computed  from  inverse  dynamics 
with  zero  frictional  forces  and  zero  joint  accelerations. 
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7.8  The  Modeling  of  Dissipative  Forces 

Broadly  speaking,  frictional  forces  are  of  two  basic  types,  namely,  (a)  viscous  forces 
and  (b)  Coulomb,  or  dry-friction,  forces.  The  latter  occur  when  contact  between 
two  solids  takes  place  directly,  the  former  when  contact  between  the  solids  takes 
place  via  a  viscous  fluid,  e.g.,  a  lubricant.  In  the  analysis  of  viscous  fluids,  a  basic 
assumption  is  that  the  relative  velocity  between  the  fluid  and  the  solid  vanishes 
at  the  fluid-solid  interface,  i.e.,  at  the  solid  boundary  confining  the  fluid.  Hence, 
a  velocity  gradient  appears  within  the  fluid,  which  is  responsible  for  the  power 
dissipation  inside  it.  In  fact,  not  all  the  velocity  gradient  within  the  fluid,  but  only  its 
symmetric  part,  is  responsible  for  power  dissipation;  the  skew-symmetric  part  of  the 
velocity  gradient  accounts  for  a  rigid-body  rotation  of  a  small  fluid  element.  Thus, 
if  a  velocity  field  v(r,  is  defined  within  a  region  IZ  occupied  by  a  viscous  fluid, 
for  a  point  of  the  fluid  of  position  vector  r  at  a  time  t,  then,  the  velocity  gradient 
grad(v)  =  9v/9r,  can  be  decomposed  as 

grad(v)  =  D  + W  (7.124) 

where  D  and  W  are  the  symmetric  and  the  skew- symmetric  parts  of  the  velocity 
gradient,  i.e., 

D  =  i[grad(v)  +  grad^(v) ],  W  =  i[grad(v)  -  grad^(v) ]  (7.125) 

The  kinematic  interpretation  of  D  and  W  is  given  below:  The  former  accounts  for 
a  distorsion  of  an  infinitesimally  small  spherical  element  of  fluid  into  a  three-axis 
ellipsoid,  the  ratios  of  the  time  rates  of  change  of  the  lengths  of  the  three  axes  being 
identical  to  the  ratios  of  the  real  eigenvalues  of  D;  the  latter  accounts  for  the  angular 
velocity  of  the  ellipsoid  as  a  rigid-body.  Clearly,  both  D  and  W  change  from  point 
to  point  within  the  fluid  and  also  from  time  to  time,  i.e., 

D  =  D(r,  t),  W  =  W(r,  t)  (7.126) 

Since  the  skew- symmetric  matrix  W  accounts  only  for  the  rotation  of  a  dif¬ 
ferential  element  of  fluid  as  a  rigid  body,  it  cannot  be  responsible  for  any  energy 
dissipation,  and  hence,  the  only  part  that  is  responsible  for  this  is  D.  In  fact,  for  a 
linearly  viscous,  incompressible  fluid  of  viscosity  coefficient  /x,  the  power  dissipated 
within  IZ  is  given  by 


=  /  iiiv{YV)dn  (i.ni) 

Jn 

Now,  if  the  motion  of  the  lubricant  separating  the  two  cylindrical  surfaces  of  a 
revolute  pair  is  modeled  as  a  purely  tangential  velocity  field  (Currie  1993),  which 
assumes  that  the  two  cylinders  remain  concentric,  then  the  foregoing  expression  for 
leads  to  the  dissipation  function 
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(7.128) 


where  0  is  the  relative  angular  speed  between  the  two  cylinders  and  the  coefficient 
^  is  a  function  of  the  lubricant  viscosity  and  the  geometry  of  the  kinematic  pair  at 
hand.  If  the  kinematic  pair  under  study  is  prismatic,  then  we  can  model  the  motion 
of  the  lubricant  between  the  two  prismatic  surfaces  as  a  Couette  flow  between  a 
pair  of  parallel  surfaces  of  the  sides  of  the  prism.  Under  these  conditions,  then,  the 

associated  dissipation  function  A  takes  on  the  same  form  of  that  given  for  a  revolute 

•  • 

pair  in  Eq.  (7.128),  in  which  the  sole  difference  is  that  0  changes  to  b,  the  time  rate 
of  change  of  the  associated  joint  variable.  Of  course,  b  is  the  relative  speed  between 
the  two  prismatic  surfaces.  Thus  in  any  event,  the  dissipation  function  of  the  /th 
joint  due  to  linearly  viscous  effects  can  be  written  as 

A,-  =  (7.129) 

•  • 

where  Ot  changes  to  bi  if  the  /th  pair  is  prismatic.  The  dissipation  function  thus 
arising  then  reduces  to 


\  T 

A  =  ^  A,  =  (7.130) 

1 

where  the  constant  n  xn  matrix  B  is  given  by 

B  =  diag(^i,^2,...,i6«)  (7.131) 

and  hence,  the  generalized  force  5  ^  associated  with  linearly  viscous  effects  is  linear 
in  the  vector  of  joint  rates,  ^ ,  i.e.. 


S 


(7.132) 


and  so,  A  =  — (l/2)n^. 

Coulomb,  or  dry  friction,  is  much  more  difficult  to  model.  If  denotes  either 
the  dissipative  torque  produced  by  Coulomb  friction  at  a  revolute  or  the  dissipative 
force  produced  by  Coulomb  friction  at  a  prismatic  joint,  and  Of  the  associated  joint 
rate,  then,  the  simplest  model  for  the  resulting  generalized  Coulomb-friction  force  is 

8f  =  -T,^sgn(4)  (7.133) 

where  sgn(-)  denotes  the  signum  function,  which  is  defined  as  +1  or  —1,  depending 
on  whether  its  argument  is  positive  or  negative,  and  is  a  positive  constant 
representing  a  torque  for  revolute  joints  or  a  force  for  prismatic  joints.  The  numerical 
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value  of  this  constant  is  to  be  determined  experimentally.  The  foregoing  model  leads 
to  a  simple  expression  for  the  associated  dissipation  function,  namely, 

Af  =  Tf\9i\  (7.134) 


The  Coulomb  dissipation  function  for  the  overall  manipulator  is,  then, 

n 

=  (7.135) 

1 

The  foregoing  simplified  model  of  Coulomb  friction  forces  is  applicable  when 
the  relative  speed  between  the  two  surfaces  in  contact  is  high.  However,  at  low 
relative  speed,  that  model  becomes  inaccurate.  In  robotics  applications,  where 
typical  end-effector  maximum  speeds  are  of  the  order  of  1  m/s,  relative  speeds 
are  obviously  low,  and  hence,  a  more  accurate  model  should  be  introduced.  Such 
a  model  should  account  for  the  empirical  observation  that  Coulomb  frictional  forces 
are  higher  at  low  relative  speeds  and  become  constant  at  very  high  relative  speeds. 
A  model  taking  this  fact  into  account  has  the  form 

3^  =  -(rf  +  (7.136) 


where  y, ,  and  €i  are  constants  associated  with  the  /th  joint  and  are  to  be  determined 
experimentally.  The  foregoing  expression  readily  leads  to  the  dissipation  function 
associated  with  the  same  joint,  namely, 

Af  =  rflOil  +  —  (1  (7.137) 

Yi 

and  hence,  the  Coulomb  dissipation  function  of  the  overall  manipulator  becomes 


A 


c 


n 


E 


1  >- 


T- 


c 


Yi 


(7.138) 


Dissipation  functions  are  very  useful.  On  the  one  hand,  they  allow  us  to  obtain 
associated  generalized  frictional  forces  when  these  are  difficult,  if  not  impossible, 
to  express  in  formula  form.  On  the  other  hand,  since  dissipation  functions  represent 
nonrecoverable  forms  of  power,  their  integrals  over  time  yield  the  dissipated  energy. 
Moreover,  the  energy  dissipated  into  unrecoverable  heat  can  be  estimated  from  an 
energy  balance,  and  hence,  the  parameters  associated  with  that  dissipation  function 
can  be  estimated  with  suitable  identification  techniques,  once  a  suitable  model  for 
a  dissipation  function  is  available.  Furthermore,  the  said  parameters  appear  in  the 
generalized  frictional  forces  as  well.  For  this  reason,  knowing  these  parameters  is 
essential  for  the  modeling  of  the  corresponding  generalized  frictional  forces. 
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7.9  Exercises 


7.1  Show  that: 

(a)  the  6/7 -dimensional  manipulator  twist  lies  in  the  null  space  of  the  6/7  x  6/7 
manipulator  angular  velocity  matrix  W ; 

(b)  the  time-derivative  of  the  6/7  x  6/7  manipulator  mass  matrix  M  is  given  by 


M  =  WM  -  MW 


(c) 

dll 

-P  =  Mt  +  WMt 

dt 

thereby  verifying  Eq.  (7.15). 

7.2  In  order  to  gain  insight  into  the  meaning  of  vector  y,  as  defined  in  Exam¬ 
ple  7.3.1,  we  define  a  similar  vector  rj  as 


Compute  Tj  for  that  example  and  compare  the  result  with  y . 

7.3  The  decoupled  robot  of  Eig.  4.23  is  to  undergo  a  maneuver,  at  the  posture  dis¬ 
played  in  that  figure,  that  involves  the  velocity  and  acceleration  specifications 
given  below,  in  base  coordinates: 


c  = 


c  = 


T' 

0 

.1. 

O' 

1 

0 


m/s,  (o  = 


O' 

1 

0 


rad/s, 


m/s^,  (i)  = 


'I 

0 

1 


r 

rad/  s' 


Compute  the  joint  torques  required  to  drive  the  robot  through  the  desired 
maneuver,  if  the  robot  is  known  to  have  the  inertial  parameters  given  below: 


m\  =  10.521,  m2  =  15.781,  =  8.767, 

m\  =  1.052,  ms  =  1.052,  m^  =  0.351 
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Fig.  7.9  Three  different  pairs  of  coupled  bodies 
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Ii  =  diag  [1.6120  0.5091  1.6120] 
h  =  diag  [0.4898  8.0783  8.2672] 

13  =  diag  [3.3768  0.3009  3.3768] 

14  =  diag  [O.I8IO  0.1810  0.1273] 

15  =  diag  [0.0735  0.0735  0.1273] 
le  =  diag  [0.0071  0.0071  0.0141] 

where  m,  ,  />,  ,  and  I,  are  given  in  units  of  kg,  m  and  kg  m^,  respectively,  with 
the  position  vectors  of  the  centers  of  mass  and  the  moment-of-inertia  matrices 
given  in  link-fixed  coordinates.  Note:  Assume  that  Z-j  is  perpendicular  to  Z5 
and  Z6,  with  O7  located  at  the  OP  of  the  EE. 

7.4  Derive  homogeneous,  linear  constraint  equations  on  the  twists  of  the  pairs  of 
coupled  bodies  appearing  in  Fig.  7.9,  namely, 

(a)  two  rigid  pulleys  coupled  by  an  inextensible  belt,  under  no  slip; 

(b)  the  bevel  pinion-and-gear  train  with  axes  intersecting  at  an  arbitrary 
angle  a\ 
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Fig.  7.10  A  three-revolute  spherical  wrist 


(c)  the  cam-and-follower  mechanism  whose  cam  disk  is  an  eccentric  circular 
disk. 

Notice  that  the  constraint  equations  sought  should  have  the  form: 

Ati  “h  Bt2  —  0 

with  ti  and  t2  denoting  the  twists  of  bodies  1  and  2,  respectively. 

7.5  Use  the  expressions  derived  in  Example  7.6.2  with  the  aid  of  the  natural 
orthogonal  complement,  as  pertaining  to  the  planar  manipulator  of  Fig.  7.1, 
to  obtain  an  expression  for  the  time-derivative  of  the  inertia  matrix  of  this 
manipulator.  Compare  the  expression  thus  obtained  with  that  derived  in 
Example  7.3.1,  and  verify  that  the  difference  I  —  2C  is  skew- symmetric — 
see  Exercise  12.2 — where  C  is  the  matrix  coefficient  of  the  Coriolis  and 
centrifugal  terms. 

7.6  A  three-revolute  spherical  wrist  with  an  orthogonal  architecture,  i.e.,  with 
neighboring  joint  axes  at  right  angles,  is  shown  in  Fig.  7.10.  Assume  that 
the  moments  of  inertia  of  its  three  links  with  respect  to  O,  the  point  of 
concurrency  of  the  three  axes,  are  given  by  constant  diagonal  matrices,  in  link- 
fixed  coordinates,  as 


I4  =  diag(7i,  J2,  J3) 
h  =  diag(Ku  K2,  K3) 
le  =  diag(Lu  L2,  L3) 
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Fig.  7.11  A  two-revolute  pointing  manipulator 

while  the  potential  energy  of  the  wrist  is 

V  =  —m^ga  cos  Os 

Moreover,  the  motors  produce  torques  14,  rs,  and  15,  respectively,  whereas  the 
power  losses  can  be  accounted  for  via  a  dissipation  function  of  the  form 


where  bt  and  for  /  =  4,  5,  6,  are  constants. 

(a)  Derive  an  expression  for  the  matrix  of  generalized  inertia  of  the  wrist. 

(b)  Derive  an  expression  for  the  term  of  Coriolis  and  centrifugal  forces. 

(c)  Derive  the  dynamical  model  of  the  wrist.  Hint:  The  kinetic  energy  T  of  a 
rigid  body  rotating  about  a  fixed  point  O  with  angular  velocity  (o  can  be 
written  as  T  =  ^co^loco,  where  Iq  is  the  moment- of- inertia  matrix  of  the 
body  with  respect  to  O. 

7.7  Shown  in  Fig.  7.11  is  a  two-revolute  pointing  manipulator.  The  centroidal 
inertia  matrices  of  the  links  are  denoted  by  Ii  and  I2.  These  are  given,  in 
link- fixed  coordinates,  by: 


■/ll 

hi  hi 

1 

(N 

1 _ 

Il  = 

hi 

hi  hi 

,  h  = 

Jii  J21  hi 

-hi 

hi  hi_ 

_Jii  hi  hi_ 

Moreover,  the  centers  of  mass  of  the  links  are  denoted  by  Ci  and  C2, 
respectively,  and  are  shown  in  the  same  figure,  the  masses  being  denoted  by 
mi  and  m2. 
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(a)  Determine  the  kinetic  energy  of  the  manipulator  as  a  quadratic  function 
of  di  and  62. 

(b)  Determine  the  2x2  matrix  of  generalized  inertia. 

(c)  Find  an  expression  for  the  time-rate  of  change  of  the  matrix  of  generalized 
inertia  by  straightforward  differentiation  of  the  expression  found  in 
item  (b). 

(d)  Repeat  item  (c),  but  now  by  differentiation  of  the  three  factors  of  I,  as 
given  in 


I  =  T^MT 

7.8  The  twist  t,  of  the  i  th  link  of  an  «-dof  serial  manipulator  can  be  expressed  as 

t,  =  T,^ 

where  T/  is  a  6xw  link-twist-shaping  matrix  and  0  is  the  w -dimensional  vector 
of  actuated  joint  rates.  Moreover,  let  Mf  and  W/  be  the  6x6  matrices  defined 
in  Sect.  7.3.  Show  that  if  the  link  is  constrained  to  undergo  planar  motion,  then 
the  product  Tf  W/M/T/  vanishes. 

7.9  Devise  a  recursive  algorithm  to  compute  the  joint  torques  required  to  balance 
a  wrench  w  acting  at  the  EE  of  a  six-revolute  manipulator  of  arbitrary  archi¬ 
tecture.  Then,  derive  the  number  of  floating-point  operations  (multiplications 
and  additions)  required  to  compute  these  torques,  and  compare  your  result 
with  the  number  of  floating  point  operations  required  to  compute  the  same  by 
matrix-times-vector  multiplications,  using  the  transpose  Jacobian. 

7.10  Establish  the  computational  cost  incurred  in  computing  the  term  of  Coriolis 
and  centrifugal  forces  of  an  -revolute  serial  manipulator,  when  the  Newton- 
Euler  algorithm  is  used  for  this  purpose. 

7.11  Shown  in  Fig.  7.12  is  an  RRP  manipulator,  whose  DH  parameters  are  dis¬ 
played  in  Table  7.5.  The  masses  of  its  three  moving  links  are  denoted  by  mi, 
m2,  and  m3,  and  the  center  of  mass  of  each  of  links  1  and  2  coincides  with  Oi, 
while  the  center  of  mass  of  link  3  is  located  at  P .  Moreover,  the  centroidal 
moments  of  inertia  of  these  links  are,  in  link-fixed  coordinates. 


[ll]2  =  Al,  [l2]3  =  Bl,  [I3]4  =  C1 


where  1  denotes  the  3x3  identity  matrix. 

(a)  Derive  the  Euler-Lagrange  equations  of  the  manipulator  under  the 
assumption  that  gravity  acts  in  the  direction  of  Xi. 

(b)  Find  the  generalized  inertia  matrix  of  the  manipulator. 

7.12  A  link  is  said  to  be  inertially  isotropic  if  its  three  principal  moments  of  inertia 
are  identical. 
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Fig.  7.12  An  RRP  spatial  manipulator 


Table  7.5  DH  parameters  of 
the  RRP  manipulator 
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O 

(a)  Show  that  any  direction  is  a  principal  axis  of  inertia  of  an  inertially 
isotropic  link. 

(b)  Explore  the  advantages  of  a  manipulator  with  inertially  isotropic  links 
with  regard  to  its  real-time  control,  i.e.,  find  the  savings  in  floating-point 
operations  required  to  compute  the  recursive  Newton-Euler  algorithm  of 
such  a  manipulator. 

7.13  Devise  an  algorithm  similar  to  Algorithm  7.6.1,  but  applicable  to  planar 
manipulators,  and  determine  the  computational  costs  involved  in  its  imple¬ 
mentation. 

7.14  Write  a  piece  of  code  to  evaluate  numerically  the  inertia  matrix  of  an  n- 
axis  manipulator  and  test  it  with  the  manipulator  of  Example  7.6.2.  Eor  this 
purpose,  assume  that  I  =  ma^. 

7.15  With  reference  to  the  mathematical  model  of  a  /7-dof  serial  manipulator  of 
Eq.  (7.60),  show  that  the  matrix  difference  A  =  I  —  2C  is  skew- symmetric. 
This  result  is  important,  because  it  leads  to  the  stabilization  of  the  manipulator 
with  the  aid  of  a  simple  proportional-derivative  (PD)  controller  (Spong  et  al. 
2006). 


Chapter  8 

Special  Topics  in  Rigid-Body  Kinematics 


8.1  Introduction 

The  motivation  for  this  chapter  is  twofold.  On  the  one  hand,  the  determination  of 
the  angular  velocity  and  angular  acceleration  of  a  rigid  body  from  point- velocity 
measurements  is  a  fundamental  problem  in  kinematics.  On  the  other  hand,  the 
solution  of  this  problem  is  becoming  increasingly  relevant  in  the  kinematics  of 
parallel  manipulators,  to  be  studied  in  Chap.  10.  Moreover,  the  estimation  of  the 
attitude  of  a  rigid  body  from  knowledge  of  the  Cartesian  coordinates  of  some  of  its 
points  is  sometimes  accomplished  by  time-integration  of  the  velocity  data.  Likewise, 
the  use  of  accelerometers  in  the  area  of  motion  control  readily  leads  to  estimates  of 
the  acceleration  of  a  sample  of  points  of  a  rigid  body,  which  can  be  used  to  estimate 
the  angular  acceleration  of  the  body,  and  hence,  to  better  control  its  motion. 

In  order  to  keep  the  discussion  at  the  level  of  fundamentals,  we  assume 
throughout  this  chapter  that  the  information  available  on  point  velocity  and  point 
acceleration  is  error-free,  a  rather  daring  assumption,  but  useful  for  understanding 
the  underlying  concepts  at  this  level.  Once  the  fundamentals  are  well  understood, 
devising  algorithms  that  yield  the  best  estimates  of  angular  velocity  and  acceleration 
in  the  presence  of  noisy  measurements  becomes  an  easier  task.  For  the  sake  of 
conciseness,  the  problem  of  motion  estimation  will  not  be  discussed  in  this  book. 


8.2  Computation  of  Angular  Velocity  from  Point- Velocity 
Data 

The  twist  of  a  rigid  body,  as  introduced  in  Eq.  (3.72),  defines  completely  the  velocity 
field  of  a  rigid  body  under  arbitrary  motion.  Notice  that  the  twist  involves  two  vector 
quantities,  the  angular  velocity  and  the  velocity  of  a  point  of  the  rigid  body.  Since 
we  are  assuming  that  point- velocity  data  are  available,  the  only  item  to  be  computed 
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is  the  angular  velocity  of  the  body  under  study,  which  is  the  subject  of  this  section. 
Once  the  angular  velocity  is  known  and  the  velocities  of  a  set  of  body  points  are 
available,  other  relevant  motion  parameters,  such  as  the  location  of  the  ISA — see 
Sect.  3.4 — can  be  readily  determined. 

If  the  twist  of  a  rigid  body  is  known,  the  computation  of  the  velocity  of  an 
arbitrary  point  of  the  body,  of  a  given  position  vector,  is  straightforward.  However, 
the  inverse  problem,  namely,  the  computation  of  the  twist  of  the  motion  under  study 
given  the  velocities  of  a  set  of  points  of  known  position  vectors,  is  a  more  difficult 
task.  A  solution  to  this  problem  is  now  outlined. 

First  and  foremost,  we  acknowledge  that  the  velocities  of  a  minimum  of  three 
noncollinear  points  are  needed  in  order  to  determine  the  angular  velocity  of  the  rigid 
body  under  study.  Indeed,  if  the  velocity  of  a  single  body  point  is  known,  we  have 
no  information  on  the  angular  motion  of  the  body;  if  the  velocities  of  two  points  are 
known,  we  can  calculate  two  components  of  the  angular- velocity  vector  of  the  body, 
namely,  those  that  are  orthogonal  to  the  line  joining  the  two  given  points,  thereby 
leaving  one  component  indeterminate,  the  one  along  that  line.  Therefore,  in  order  to 
know  the  angular  velocity  of  a  rigid  body  in  motion,  we  need  at  least  the  velocities 
of  three  noncollinear  points  of  the  body — obviously,  knowing  only  the  velocities  of 
any  number  of  points  along  one  line  yields  no  more  information  than  knowing  only 
the  velocities  of  two  points  along  that  line.  We  thus  assume  henceforth  that  we  have 
three  noncollinear  points  and  that  we  know  perfectly  their  velocities. 

Let  the  three  noncollinear  points  of  the  body  under  study  be  denoted  by  {Pi 
and  let  { p/  be  their  corresponding  position  vectors.  The  centroid  C  of  the 
foregoing  set  has  a  position  vector  c  that  is  the  mean  value  of  the  three  given  position 
vectors,  namely. 


c=l^P,  (8.1) 

1 

Likewise,  if  the  velocities  of  the  three  points  are  denoted  by  p/ ,  and  that  of  their 
centroid  by  c,  one  has 

1  3 

c  =  -  L!  P' 

1 

From  Eq.  (3.49),  the  velocity  of  the  three  given  points  can  be  expressed  as 

p^-  =  c  + S^(p^ -c),  /  =  1,2,3  (8.3a) 


or 


p,  -  c  =  -c),  i  =  1,2,3 


(8.3b) 
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Now,  we  define  a  3  x  3  matrix  P  as 

P  =  [pi  -  c  P2  -  c  P3  -  c]  (8.4) 

upon  differentiation  of  both  sides  of  Eq.  (8.4)  with  respect  to  time,  one  has 

i*  =  [pi  -  c  P2  -  c  P3  -  c]  (8.5) 

It  is  noteworthy  that  P  and  P  are  immutable  under  a  pure  translation  of  the  coordinate 

frame  of  reference.  However,  under  a  pure  rotation  of  the  frame,  given  by  a  proper 

•  • 

orthogonal  matrix  Q,  both  P  and  P  transform  as  QP  and  QP,  respectively.  As  a 
consequence,  P  and  P  are  not  frame  invariant. 

Further,  Eqs.  (8.3b)  can  be  written  in  matrix  form  as 

F  =  ^F  (8.6) 

from  which  we  want  to  solve  for  Q, ,  or  equivalently,  for  (o .  This  cannot  be  done  by 
simply  multiplying  by  the  inverse  of  P,  because  the  latter  is  a  singular  matrix.  In 
fact,  as  the  reader  can  readily  verify,  any  vector  having  three  identical  components 
lies  in  the  null  space  of  P,  thereby  showing  that  P  is  singular,  its  null  space  being 
spanned  by  that  vector.  Furthermore,  notice  that  from  Eq.  (8.3b),  it  is  apparent  that 

(p,  -  cfo)  =0,  /  =  1, 2, 3  (8.7a) 

Upon  assembling  all  three  scalar  equations  above  in  one  single  vector  equation,  we 
obtain 


=  0 


(8.7b) 


a  result  that  is  summarized  below: 

Theorem  8.2.1.  The  angular-velocity  vector  lies  in  the  null  space  of  matrix  F^ , 
with  P  defined  in  Eq.  (8.5). 

In  order  to  find  the  desired  expression  for  (o  from  the  above  equation,  we  recall 
Theorem  A.l,  which  is  proven  in  Appendix  A:  Let  S  be  a  skew- symmetric  3x3 
matrix  and  A  be  an  arbitrary  3x3  matrix.  Then, 

vect(SA)  =  -  [tr(A)l  —  A]  vect(S)  (8.8) 

Upon  application  of  the  foregoing  result,  Eq.  (8.6)  leads  to 


Do)  =  vect(P) 


(8.9) 
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where  D  is  defined  below  and  vect(S^)  is  nothing  but  w: 

i[tr(P)l-P]  (8.10) 

Thus,  Eq.  (8.9)  can  be  solved  for  (o  as  long  as  D  is  invertible.  It  is  to  be  expected 
that,  if  the  three  points  are  collinear,  then  D  is  invertible,  but,  given  that  P  is  not 
frame-invariant,  neither  is  tr(P).  Hence,  it  is  not  apparent  from  Eq.  (8.10)  that  D  is 
singular  when  the  three  given  points  are  collinear.  We  will  discuss  this  singularity 
presently. 

Now,  if  tr(P)  vanishes,  D  becomes  just  one-half  the  negative  of  P,  which,  as  we 
saw  above,  is  singular.  Moreover,  if  the  three  given  points  are  noncollinear  and  we 
assume  that  the  trace  of  P  does  not  vanish,  then  the  inverse  of  D  can  be  proven  to  be 

D“‘=a'l-|6p2  (8.11) 


where  coefficients  a  and  P  are  given  below: 


a  = 


2 

fiW’ 


tr(P)[tr(p2)  -  tr2(P)] 


(8.12) 


Erom  expressions  (8.12)  it  is  apparent  that  D  fails  to  be  invertible  not  only  when 
tr(P)  vanishes,  but  also  when  the  term  in  brackets  in  the  denominator  of  P  does.  In 
Exercise  8.3,  the  reader  is  asked  to  prove  that  the  foregoing  term  vanishes  whenever 
the  three  points  are  collinear. 

Erom  the  foregoing  discussion,  it  is  apparent  that  given  the  velocities  and  the 
position  vectors  of  three  noncollinear  points  of  a  rigid  body,  the  angular  velocity 
of  the  body  can  always  be  determined.  However,  the  data,  i.e.,  the  velocities  of 
the  three  given  points,  cannot  be  arbitrary,  for  they  must  conform  to  Eq.  (8.6)  or, 
equivalently,  to  Theorem  8.2.1.  Equation  (8.6)  states  that  the  columns  of  matrix  P 

must  lie  in  the  range  of  ,  while  Theorem  8.2.1  states  that  o)  lies  in  the  null  space  of 

• 

P.  However,  prior  to  the  computation  of  o) ,  or  equivalently,  of  ,  it  is  not  possible 
to  verify  this  condition.  An  alternative  approach  to  verifying  the  compatibility  of  the 
data  follows:  Since  lines  Pi  C  belong  to  a  rigid  body,  vectors  p,  —  c  must  remain  of 
the  same  magnitude  throughout  a  rigid-body  motion.  Moreover,  the  angles  between 
any  two  of  the  said  lines  must  be  preserved  throughout  the  motion  as  well.  This 
means  that  the  conditions  below  must  hold: 


(P/  -  c)^(py  -  c)  =  Cij ,  i,  j  =  1,2,3  (8.13) 


or  in  compact  form, 


P^P  =  C 


(8.14) 
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where  the  (/ ,  j)  entry  of  the  constant  matrix  C  is  Cij ,  as  defined  in  Eq.  (8.13)  above. 
Upon  differentiation  of  both  sides  of  Eq.  (8.14)  with  respect  to  time,  we  obtain: 

Theorem  8.2.2  (Velocity  Compatibility).  The  velocities  of  three  points  of  a  rigid 
body  satisfy  the  compatibility  condition: 

P^P  +  P^P  =  0  (8.15) 

with  matrices  P  and  P  defined  in  Eqs.  (8.4)  and  (8.5)  and  O  denoting  the  3x3  zero 
matrix. 

The  above  equation,  then,  states  that  for  the  given  velocities  of  three  points  of  a 
rigid  body  to  be  compatible,  the  product  P^P  must  be  skew- symmetric.  Note  that 
the  above  matrix  compatibility  equation  represents  six  independent  scalar  equations 
that  the  data  of  the  problem  at  hand  must  satisfy.  There  is  a  tendency  to  neglect 
the  foregoing  six  independent  scalar  compatibility  conditions  and  to  focus  only  on 
the  three  scalar  conditions  drawn  from  the  diagonal  entries  of  the  above  matrix 
equation.  This  is,  however,  a  mistake,  for  these  three  conditions  do  not  suffice  to 
guarantee  data  compatibility  in  this  context;  all  these  three  conditions  guarantee  is 
that  the  distance  between  any  pair  of  points  of  the  set  remains  constant,  but  they  say 
nothing  about  the  angles  between  the  pairs  of  lines  formed  by  each  pair  of  points. 

Note,  on  the  other  hand,  that  the  product  PP^  has  no  direct  geometric  interpre¬ 
tation,  although  the  difference  tr(PP^)l  —  PP^  does,  as  discussed  in  Exercise  8.9. 
Eurthermore,  while  Theorem  8.2.2  states  that  matrix  P^P  is  skew- symmetric,  it  says 
nothing  about  the  product  PP^ .  All  we  can  say  about  this  product  is  stated  in  the 
result  below: 

Theorem  8.2.3.  With  matrices  P  and  P  defined  in  Eqs.  (8.4)  and  (8.5),  the  product 
PP  obeys  the  constraint 


tr(PP^)  =  0  (8.16) 

If  m  X  /I  matrices  are  regarded  as  forming  a  vector  space,  then  an  inner  product  of 
two  such  matrices  A  and  B,  denoted  by  (A,  B),  can  be  defined  as 

(A,  B)  =  tr(AB^)  (8.17) 

the  two  matrices  being  said  to  be  orthogonal  when  the  foregoing  inner  product 
vanishes.  We  thus  have  that  Theorem  8.2.3  states  that  matrices  P  and  P  are 
orthogonal,  a  result  that  parallels  that  about  the  orthogonality  of  the  relative  velocity 
of  two  points  and  the  line  joining  them,  as  stated  in  Eq.  (3.51)  and  summarized  in 
the  ensuing  theorem.  The  proof  of  Theorem  8.2.3  is  left  as  an  exercise. 

Example  8.2.1.  The  rigid  cube  shown  in  Eig.  8.1  moves  in  such  a  way  that  vertices 
Pi,  P2,  and  P3  undergo  the  velocities  shown  in  that  figure,  for  three  different 
possible  motions.  The  length  of  the  sides  of  the  cube  is  1,  and  the  velocities  all  have 
magnitude  \/2  in  Eig.  8.1a,  c;  these  velocities  are  of  unit  magnitude  in  Eig.  8.1b. 
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Fig.  8.1  A  rigid  cube  undergoing  a  motion  determined  by  the  velocities  of  three  of  its  points 


Furthermore,  in  the  motion  depicted  in  Fig.  8.1c,  the  velocity  of  is  parallel  to 
line  PaP?,,  whereas  that  of  P2  is  parallel  to  line  PiP^-  Out  of  the  three  different 
motions,  it  is  known  that  at  least  one  is  compatible.  Identify  the  compatible  motion 
and  compute  its  angular  velocity. 

Solution:  Let  p/  denote  the  velocity  of  Pi ,  of  position  vector  p/ .  Each  proposed 
motion  is  then  analyzed:  (a)  The  projection  of  pi  onto  Pi  F*2  is  1,  but  that  of  p2  onto 
the  same  line  is  0,  and  hence,  this  motion  is  incompatible;  (b)  Again,  the  projection 
of  Pi  onto  P1P2  is  1,  but  that  of  p2  onto  the  same  line  vanishes,  and  hence,  this 
motion  is  also  incompatible.  Thus,  the  only  possibility  is  (c),  which  is  now  analyzed 
more  formally:  Use  a  dextrous — right-handed — rectangular  coordinate  frame  with 
origin  at  Pi,  axis  Y  along  P1P2,  and  axis  Z  parallel  to  F*2^3-  All  vectors  and 
matrices  are  now  represented  in  this  coordinate  frame,  and  hence. 
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Now  matrices  P  and  P  are  constructed: 


II 

0 

0 

0 
_ 1 

1 

U) 

0 

1 

U) 

_J 

-2  1  1 

1  1  -2 

J 

_-l  -1  2_ 

J 

1 

(N 

1 

_ 1 

Furthermore, 


P^P 


1 

3 


■  0  -1 
1  0 
-1  1 


1 

-1 

0 


which  is  skew- symmetric,  and  hence,  the  motion  is  compatible.  Now,  matrix  D  is 
computed: 


l[ltr(P)-P] 


30  0 
22-1 


The  angular  velocity  co  is  computed  as  the  solution  to 

Do)  =  vect(P) 


where 


vect(P)  = 


1 

6 


3 

-1 

1 


Equations  (8.9)  are  thus 


3u)i  =  3 

2(j0i  +  2(02  —  CO3  =  —  1 
COl  “h  CO2  ~h  (O3  —  1 


The  first  of  the  foregoing  equations  leads  to 


COl 


1 


whereas  the  second  and  the  third  lead  to 


2(02  —(03=  —3 

(O2  -h  (O3  =  0 
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and  hence, 


CO2  =  —h  CO3  =  1 

Now,  as  a  verification,  co  should  be  normal  to  the  three  columns  of  P  as  defined 
in  Eq.  (8.7b);  in  other  words,  co  should  lie  in  the  null  space  of  P^.  But  this  is  so, 
because 


(O  = 


3  1  -2 

"  1  ■ 

1 

0 

"O' 

0  1  1 

-1 

0 

1 

(N 

1 

m 

1 

_ 1 

_  1  _ 

J 

_0_ 

thereby  verifying  that  co  lies,  in  fact,  in  the  null  space  of  P^. 


8.2.1  A  Robust  Formulation 

The  foregoing  formulation  fails  when  matrix  D  of  Eq.  (8.10)  becomes  singular. 
While  it  is  not  surprising  that  the  matrix  becomes  singular  in  the  presence  of  three 
collinear  points,  it  is  a  bit  frustrating  that,  even  if  the  points  are  noncollinear, 
D  becomes  singular  when  tr(P)  =  0.  Moreover,  in  light  of  the  lack  of  frame- 
invariance  of  P,  it  is  not  possible  to  predict  geometrically  under  which  conditions 
tr(P)  vanishes.  It  is  thus  imperative  to  look  for  an  alternative,  robust  approach,  which 
is  the  aim  of  this  subsection. 

Upon  multiplying  both  sides  of  Eq.  (8.6)  by  P^  from  the  right,  we  obtain 

PP^  =  R  =  PP^  (8.18) 

Eurther,  if  we  take  the  vector  of  both  sides  of  Eq.  (8.18),  we  obtain 

-J(o  =  vect(PP^)  (8.19a) 

where,  by  application  of  Theorem  A.  1 ,  as  done  above,  J  is  defined  as 

J  =  tr(R)l-R  (8.19b) 

which,  as  the  reader  is  invited  to  prove  in  Exercise  8.9,  is  nothing  but  the  inertia 
tensor  of  a  system  of  three  unit-mass  particles  located  at  points  {  Pi  with  respect 
to  their  mass  center,  which  coincides  with  the  centroid  C  of  the  three  given  points. 
As  such,  matrix  J  is,  in  general,  positive-definite,  becoming  semidefinite  only  in 
the  special  case  in  which  the  three  masses  are  collinear.  Hence,  the  formulation 
singularity  brought  about  by  the  vanishing  of  tr(P)  is  eliminated,  which  is  the  reason 
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why  this  formulation  is  billed  as  robust.  Hence,  as  long  as  the  three  given  points  are 
noncollinear,  Eq.  (8.19a)  can  always  be  solved  for  w,  thus  obtaining 

(O  =  2J“‘vect(PP^)  (8.20) 

Example  8.2.2.  Solve  Example  8.2. 1  with  the  robust  formulation  introduced  above. 

Solution:  We  need  both  matrix  J  and  the  right-hand  side  of  Eq.  (8.19a).  To  this 
end,  we  compute  first 


R  =  PP^ 


000 
0  2  1 
0  1  2 


which  is  apparently  a  simple,  positive-semidefinite  matrix.^  Hence, 


4  0  0 
0  2-1 
0-12 


whose  inverse  is  readily  calculated  as 


3  0  0 
0  8  4 
0  4  8 


Eurther, 


Therefore, 


vect(PP^)  =  vect  I  - 
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-3 

— 

-1 

_0  4 

8_ 

_  3  _ 

_  1  _ 

thereby  completing  the  calculations,  and  verifying  the  result  obtained  with  the  non- 
robust  formulation. 


iR  is  apparently  singular  because  it  has  one  row  and  one  column  of  zeros;  it  has  two  positive 
eigenvalues  because  its  trace  is  4/3  >  0  and  the  determinant  of  its  2  x  2  lower-right  block  is 
1/3  >  0. 
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8.3  Computation  of  Angular  Acceleration 
from  Point- Acceleration  Data 

The  angular  acceleration  of  a  rigid  body  under  general  motion  is  determined  in  this 
section  from  knowledge  of  the  position,  velocity,  and  acceleration  vectors  of  three 
noncollinear  points  of  the  body.  The  underlying  procedure  parallels  that  of  Sect.  8.2. 
Indeed,  recalling  the  notation  introduced  in  that  section,  and  letting  vectors  p/ ,  for 
i  =  1, 2,  3,  denote  the  acceleration  of  the  given  points,  one  can  rewrite  Eq.  (3.85) 
for  each  point  in  the  form 


Pi  =  c+ (ft  +  -c),  i  =  1,2,3  (8.21a) 

or 

Pi -c=  (ft +  ft2)(pi -C),  i  =  1,2,3  (8.21b) 


where  c  was  defined  in  Eq.  (8.1),  and  c  is  the  acceleration  of  the  centroid,  i.e.. 


c  = 


(8.21c) 


•  • 

Eurthermore,  matrix  P  is  defined  as 

P  =  [pi  -  c  P2  -  c  P3  -  c]  (8.22) 

Thus,  Eqs.  (8.21b)  can  be  written  in  compact  form  as 

P  =  (ft  +  ft^)P  (8.23) 

from  which  one  is  interested  in  computing  ,  or  correspondingly,  cb .  To  this  end, 
Eq.  (8.23)  is  rewritten  as 


ftp  =  W  (8.24a) 

with  matrix  W  defined  as 

W  =  P-ft^P  (8.24b) 

The  counterpart  of  Theorem  8.2.1  is  now  derived  from  Eqs.  (8.21b).  Eirst,  these 
equations  are  cast  in  the  form 


Pi  -  c  -  ft^(pi  -  c)  =  <w  X  (pi  -  c),  1  =  1 , 2, 3 
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It  is  now  apparent  that  if  we  dot-multiply  the  above  equations  by  o) ,  we  obtain 

[P;  —  c  —  —  c)]  •  o)  =  0,  /  =  1, 2,  3  (8.25a) 

Upon  assembling  the  three  foregoing  equations  in  one  single  vector  equation,  we 
derive  the  counterpart  of  Eq.  (8.7b),  namely, 

(P-fl^P)^w=0  (8.25b) 

a  result  that  is  summarized  below  in  theorem  form: 

Theorem  8.3.1.  The  angular-acceleration  vector  oo  lies  in  the  null  space  of  matrix 
W^,  with  W  defined  in  Eq.  (8.24b). 

Just  as  we  did  in  Sect.  8.2  when  solving  for  o)  from  Eq.  (8.9),  we  apply  the  result 
already  invoked  in  connection  with  Eq.  (8.9),  thereby  deriving  an  alternative  form 
of  Eq.  (8.24a),  namely. 


Dei)  =  vect(P  -  ft^P)  (8.26) 

where  D  is  defined  as  in  Eq.  (8.10).  Thus, 

<y  =  D“'vect(P  -  ft^P)  (8.27) 

with  given  as  in  Eqs.  (8.11)  and  (8.12).  As  in  Sect.  8.2,  then,  given  the  position, 
velocity,  and  acceleration  vectors  of  three  noncollinear  points  of  a  rigid  body,  it 
is  always  possible  to  compute  the  associated  angular  acceleration.  However,  as 
discussed  in  that  section,  the  data  cannot  be  given  arbitrarily,  for  they  must  comply 
with  Eq.  (8.24a),  or  correspondingly,  with  Eq.  (8.25b).  The  former  implies  that  the 
three  columns  of  matrix  W  lie  in  the  range  of  matrix  alternatively,  Eq.  (8.25b) 
implies  that  ^  lies  in  the  null  space  of  W  .  Again,  prior  to  the  determination  of  ^ , 
it  is  impossible  to  verify  this  condition,  for  which  reason  an  alternative  approach 
is  taken  to  verifying  compatibility.  The  obvious  one  is  to  differentiate  both  sides  of 
Eq.  (8.15),  which  produces 


••  rj-t  •  rj-t  •  rj-t  •• 

P^P  +  2P^P  +  P^P  =  0  (8.28) 

thereby  deriving  the  compatibility  conditions  that  the  acceleration  measurements 
should  satisfy. 

Einally,  upon  differentiation  of  both  sides  of  Eq.  (8.16)  with  respect  to  time,  and 
while  doing  this,  resorting  to  Lemma  A.2  of  Appendix  A,  we  have 


•  •  rj-i  •  •  rj~i 

tr(PP^  +  PP^)  =  0 


(8.29) 


which  is  the  counterpart  of  Eq.  (8.16  ). 
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Fig.  8.2  A  rigid  triangular 
plate  undergoing  a  motion 
given  by  the  velocity  and 
acceleration  of  its  vertices 


Example  8.3.1.  The  three  vertices  of  the  equilateral  triangular  plate  of  Fig.  8.2, 
which  lies  in  the  X-Y  plane,  are  labeled  Pi,  P2,  and  P3,  their  position  vectors 
being  pi,  P2,  and  P3.  Moreover,  the  velocities  of  the  foregoing  points  are  denoted  by 
P; ,  for  /  =  1,2,3.  The  origin  of  the  coordinate  frame  X,  Y,  Z  lies  at  the  centroid  C 
of  the  triangle,  the  velocities  of  the  vertices,  in  this  coordinate  frame,  being  given  as 


4-V2 

4 


0 

0 

1 


4-V3 

4 


0 

0 

1 


4+ 

4 


0 

0 

1 


Likewise,  pi,  p2,  and  p3  denote  the  accelerations  of  the  three  vertices  of  the  plate, 
given  below  in  the  same  coordinate  frame: 


Pi 


-6  +  4V3~ 
12-3x/2  , 

0 


8V3  +  3V6' 

3x/3 

0 


P3 


6  +  4V3  ' 

-12  +  3x/2 
0 


With  the  foregoing  information, 

(a)  show  that  the  three  given  velocities  are  compatible; 

(b)  compute  the  angular  velocity  of  the  plate; 

(c)  determine  the  set  of  points  of  the  plate  that  undergo  a  velocity  of  minimum 
magnitude; 

(d)  show  that  the  given  accelerations  are  compatible; 

(e)  compute  the  angular  acceleration  of  the  plate. 
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Solution: 

(a)  Since  the  centroid  of  the  triangle  coincides  with  that  of  the  three  given  points, 
we  have  c  =  0.  Moreover, 


1/2  - 

0 

■  -1/2  ■ 

-V3/6 

,  P2  = 

V3/3 

,  P3  = 

-V3/6 

_  0  _ 

_  0  _ 

_  0  _ 

Thus, 


Furthermore, 


and  hence. 


P 


1 

6 


3  0 

-V3  2V3 
0  0 


0 

0 

V3)/12 


0  0  0 

0  0  0 

V3-3V2 -2V3  V3 +  3V2 


We  can  readily  show  from  the  above  results  that 

P^P  =  o 


with  O  denoting  the  3x3  zero  matrix.  Hence,  matrix  P^P  is  skew-symmetric 
and  the  velocities  are  compatible 
(b)  Next,  we  have 


and 


D 


1 

2 


[tr(P)l  -  P] 


1 

12 


2V3  0  3 

V3  3 

0  0  3  +  2^/3 


1 


-2V3 

-V3  +  3^/2 
0 


vect(P)  = 


366 


8  Special  Topics  in  Rigid-Body  Kinematics 


Hence,  if  the  components  of  (o  in  the  given  coordinate  frame  are  denoted  by  coi , 
for  /  =  1,2,3,  then  we  obtain 


'2.\P^(jd\  “h  3cl)3 
+  30)2  +  V3o)3 
(3  +  2S)u)^ 


-V3 

-x/3  +  3x/2 
2 

0 


From  the  third  equation. 


Substitution  of  the  foregoing  value  into  the  first  of  the  above  equations  yields 
coi  =  —1 12.  Further,  upon  substitution  of  the  values  of  coi  and  0)3  into  the 
second  of  the  above  equations,  we  obtain  0)2  =  \/2/2  and  hence, 

-r 

V2 

0  _ 

(c)  Let  Pq  be  the  position  vector  of  the  point  Pq  on  the  instantaneous  screw  axis 
lying  closest  to  the  origin.  Now,  in  order  to  find  Pq,  we  can  resort  to  Eq.  (3.70), 
using  point  C  as  a  reference,  i.e.,  with  c  and  c  playing  the  roles  of  a  and  a  in 
that  equation.  Moreover,  since  c  =  0,  the  expression  for  Pq  reduces  to 


where  from  item  (b). 


0) 


3 

4 


while 


and  hence. 


= 


12- V3 
24 


1 

0 


12- 

18 


1 

0 


8.3  Computation  of  Angular  Acceleration  from  Point- Acceleration  Data 


367 


As  a  verification,  Pq  should  be  perpendicular  to  the  ISA,  as  it  is,  for  the  product 
to  vanish.  Next,  the  vector  representing  the  direction  of  the  screw  axis  is 
obtained  simply  as 


(0 


e  = 


V3 


(O 


[-1  V2  0] 


T 


thereby  defining  completely  the  instant  screw  axis. 

(d)  The  acceleration  of  the  centroid  of  the  three  given  points  is  given  as  follows: 


c=[- 


V6  V3 


24  ’ 


24 


,  0] 


rj-i  ••  ••  rj~i 

Then,  matrices  P,  P  P,  P  P,  and  P  P  are  readily  computed  as 


T' 


rj~i  •  • 

P^P  = 


1 

144 


•  •  rj~i 

P^P  = 


1 

144 


-6  +  4V3  +  V6 

-8^3 -2^6  6  +  4V3  +  V6 

I2-3V2+  V3 

-2  Vs  -I2  +  3V2+V3 

0 

0 

0 

-21  +6V6 

6-24V3-6V6 

15  +  24V3 

1 

6  +  24V^-6V6 

-12  6 

-24V3  +  6V6 

15-24V3 

6  +  24V3  +  6V6 

-21  -  6V6 

J 

-21  +  6x/6 

6  +  24V3-6V6 

15-24V3 

1 

6-24V3-6V6 

-12  6 

+  24V3  +  6V6 

15  +  24x/3 

6-24V3  +  6V6 

1 

(N 

1 

•  rj~i  • 

P^P  = 


1 

144 


21-6V6-6  +  6V6  -15 

-6  +  6^6  12  -6-6V6 

-15  -6  -  6x/6  21  +  6^6 


Now,  it  is  a  simple  matter  to  verify  that 


••  rj-t  •  rj-t  •  rj-t  •• 

P^P  +  2P^P  +  P^P  =  O 


and  hence,  the  given  accelerations  are  compatible. 

(e)  is  defined  as  the  unique  skew- symmetric  matrix  whose  vector  is  co,  the  latter 
having  been  computed  in  item  (b).  Thus, 


■  0  0  V2' 

-2  -V2  0 

0  0  1 

-V2  -1  0 

2 

_-V2 -1  0  _ 

4 

1 

m 

1 

0 

0 

_ 1 
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=  3- 

24 


-6  +  V6  -2V6  6  +  V6  ' 

-3x/2  +  V3  -2x/3  3V2  +  x/3 
0  0  0 


Hence, 


••  o  1 

F-^^F  =  — 
24 


4^3  -8^3  4^3' 
12  0  -12 

0  0  0 


The  angular-acceleration  vector  is  thus  computed  from 

Do)  =  vect(P  —  ^P) 

where  D  was  computed  in  item  (b),  while 


vect(P  —  S^^P) 


1 

I2 


3 

V3 

3  +  2V3 


and  hence,  letting  cbf  denote  the  ith  component  of  o)  in  the  given  coordinate 
frame,  we  obtain 


1 

12 


(iVScbi  +  3d)3) 


1 

12 


(^'s/Sdbi  “h  2(02  4“  's/^CO^} 


1 

I2 


(3  +  2  V3)d)3 


1 

4 

V3 

12 

3  +  2V3 
12 


which  yields 


(b 


0 

0 

1 


thereby  completing  the  solution.  Note  that  6)  lies,  in  fact,  in  the  null  space  of 
matrix  (P  — 
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8.3.1  A  Robust  Formulation 

In  computing  the  angular  acceleration  vector  from  point-velocity  and  -acceleration 
data,  we  face  exactly  the  same  singularity  we  faced  when  computing  the  angular- 
velocity  vector.  We  thus  follow  the  same  robust  approach  introduced  in  Sect.  8.2.1. 
To  this  end,  we  multiply  both  sides  of  Eq.  (8.24a)  by  from  the  right,  thereby 
obtaining 


HR  =  WP^ 

with  R  defined  already  in  Eq.  (8.18).  Moreover,  from  Eq.  (8.24b), 

rj~t  •  •  rj~t 

WP^  =  PP^  - 


(8.30) 


(8.31) 


Now,  the  angular-acceleration  vector  is  computed  from  Eq.  (8.30)  upon  taking  the 
vector  of  both  sides  of  this  equation,  namely, 

-Jw  =  vect(PP^  -  (8.32) 

whence,  as  long  as  the  three  given  points  are  not  collinear,  do  is  computed  as 

«  =  2J“‘vect(PP^  -  fl^R)  (8.33) 


thereby  completing  the  intended  computation. 

Example  8.3.2.  Using  the  foregoing  robust  approach,  compute  the  angular- 
acceleration  vector  of  the  motion  undergone  by  the  plate  of  Eig.  8.2,  for  the 
point- velocity  and  -acceleration  data  given  in  Example  8.3.1.  Use  the  value  of 
(o  computed  in  that  example. 

Solution:  All  we  need  now  is  J  and  the  right-hand  side  of  Eq.  (8.32).  We  thus  have 


R  =  PP^ 


1  00 
0  1  0 
000 


1  00 
0  1  0 
0  0  2 


whence  tr(R)  =  1 ;  therefore. 
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Furthermore, 


PP 


T 


1 

8 


-2 

4- 

0 


-4  -  V2  0 
-1  0 
0  0 


while  ^  is  obtained  simply  as  CPM(a)),  i.e., 


0  0  V2' 

0  0  1 
-V2-1  0 


and  hence, 

2  V20 
-Jt.  1  0 
000 

thereby  obtaining 


=  - 
8 


PP^  -  =  - 


0-10 
1  0  0 

0  0  0 


whence. 


vect(PP^  -  ^^R) 


0 

0 


which  thus  yields 


6) 


0 

0 

1 


thereby  completing  the  required  computation. 


8.4  Exercises 

8.1  The  regular  tetrahedron  of  Fig.  3.10,  of  unit-length  edges,  moves  in  such  a 
way  that  vertex  Pi  has  a  velocity  of  unit  magnitude  directed  from  Pi  to  P4, 
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whereas  the  velocity  of  P2  is  parallel  to  edge  Define  a  coordinate  frame 
X,  7,  Z  with  origin  at  F  axis  directed  from  Pi  to  the  midpoint  M  of 
P2P?,,  and  X  axis  in  the  plane  of  Pi,  P2,  P?,  as  shown  in  that  figure.  With  the 
above  information, 

(a)  find  the  velocity  of  P2  \ 

(b)  show  that  the  velocity  of  P^  cannot  be  zero; 

(c)  if  the  velocity  of  P^  lies  in  the  Pi  P2P?  plane,  find  that  velocity; 

(d)  find  the  angular  velocity  of  the  tetrahedron; 

(e)  find  the  set  of  points  of  the  tetrahedron  undergoing  a  velocity  of  minimum 
magnitude. 

8.2  The  position  vectors  of  three  points  of  a  rigid  body,  pi,  P2,  and  p3,  as  well  as 
their  velocities,  pi,  p2,  and  p3,  are  given  below: 


"r 

"  1  ■ 

"-r 

Pi  = 

1 

,  P2  = 

-1 

II 

1 

_i_ 

_  1  _ 

_-i_ 

"r 

"  3  ■ 

Pi  = 

1 

,  P2  = 

1 

II 

1 

_i_ 

_-l_ 

_  3  _ 

(a)  Is  the  motion  possible? 

(b)  If  the  motion  is  possible,  find  its  angular  velocity. 

8.3  For  matrix  P  defined  as  in  Eq.  (8.4),  i.e.,  as 

P  =  [pi  -  C  P2  -  C  P3  -  c] 

where  {  p^  are  the  position  vectors  of  three  points  of  a  rigid  body,  while  c 
is  that  of  their  centroid,  prove  that  tr(P^)  =  tr^  (P)  whenever  the  three  given 
points  are  collinear.  Is  the  converse  true? 

8.4  With  matrix  P  defined  as  in  Exercise  8.3  above,  prove  Theorem  8.2.3.  That  is, 
prove  that 

tr(PP^)  =  0 

8.5  With  the  notation  of  Sect.  8.3,  prove  that 

vect(S^^P)  =  Do) 

8.6  Derive  the  velocity  and  acceleration  compatibility  conditions  for  a  body  that 
is  known  to  undergo  spherical  motion,  i.e.,  a  motion  under  which  one  point  of 
the  body  remains  fixed. 
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8.7  The  position  vectors  of  three  points  of  a  rigid  body,  pi,  p2,  and  p3,  are  given 
as  in  Exercise  8.2,  and  repeated  below  for  quick  reference: 


"r 

"  1  ■ 

"-r 

1 

,  P2  = 

-1 

,  P3  = 

1 

_i_ 

_  1  _ 

_-i_ 

Now,  the  velocities  of  these  points  are  all  zero,  while  their  accelerations  are 
given  as 


"r 

■  3  ■ 

"-r 

•  • 

Pi  = 

1 

_i_ 

•  • 

,  P2  = 

1 

_-l_ 

•  • 

,  P3  = 

1 

_  3  _ 

(a)  Show  that  the  motion  is  compatible. 

(b)  Find  the  angular  acceleration  of  the  body. 

8.8  With  reference  to  Example  8.2.1,  compute  the  angular  acceleration  of  the  cube 
of  Fig.  8.1c  if  p,  =  0,  for  /  =  1,2,3. 

8.9  With  the  notation  of  Sect.  8.2,  let 


R  =  PP^ 


(a)  Show  that  the  moment  of  inertia  J  of  the  three  given  points,  which  is 
identical  to  that  of  a  system  of  unit  masses  located  at  these  points,  with 
respect  to  the  centroid  C  of  the  given  points,  is 

J  =  tr(R)l  -  R 

(b)  Show  that  if  the  three  given  points  move  as  points  of  a  rigid  body 
undergoing  an  angular  velocity  oo  whose  cross-product  matrix  is  ,  then 

j  =  RS2  —  S2R 

(c)  Furthermore,  show  that  if  under  the  conditions  of  item  (b)  above,  the  set 
of  points  undergoes  an  angular  acceleration  of  cross-product  matrix  , 
then 


J  —  RS2  —  S2R  —  S^^R  —  RS^^  2S2RS2 

8.10  A  wrench  of  unknown  force  f  is  applied  to  a  rigid  body.  In  order  to  find  this 
force,  its  moment  with  respect  to  a  set  of  points  {Pk}\,  of  position  vectors 
{  p^  is  measured  and  stored  in  the  set  {  Show  that  f  can  be  calculated 
from  the  relation 
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Df  =  — vect(M) 

with  D  defined  as  in  Sect.  8.2,  i.e.,  as 

l[tr(P)l-P] 


and  M  given  by 


M  =  [ni  —  n  n2  —  n  n3  —  n] , 


n  = 


1 

3 


3 


1 


Note  that  P  is  defined  in  Exercise  8.3. 

8.11  A  wrench  is  applied  to  the  tetrahedron  of  Fig.  3.10.  When  the  force  of  this 
wrench  acts  at  point  Pk,  the  resulting  moment  is  n^,  for  =  1,2,3.  For  the 
data  displayed  below,  in  frame  T  of  that  figure,  find  the  resultant  force  f,  as 
well  as  the  line  of  action  of  this  force  that  will  lead  to  a  moment  of  minimum 
magnitude.  Determine  this  moment. 


V2 

"r 

1 

■  3V2  ■ 

1 

■  3V2  ■ 

0 

-1-/1 

1-/I 

4 

_o_ 

12 

_  2s/3  _ 

12 

_-2\/3_ 

8.12  Matrix  D,  as  defined  from  Eq.  (8.6)  and  displayed  in  Eq.  (8.10),  was  found  to 
involve  frequent  singularities,  even  in  the  presence  of  noncollinear  points.  This 
weakness  stems  from  its  lack  of  frame-invariance,  and  can  be  readily  fixed  if 
both  sides  of  Eq.  (8.6)  are  multiplied  by  from  the  right.  Show  that,  under 
these  conditions,  an  equation  similar  to  (8.9)  is  derived,  but  with  D  replaced  by 
(1/2)J,  with  J  defined  as  in  Exercise  8.9.  Now  show  that  J  is  frame-invariant 
in  the  sense  of  Sect.  2.7,  and  becomes  singular  if  and  only  if  the  three  given 
points  are  collinear. 

8.13  A  ball- wheel  is  used  to  drive  a  mobile  robot.  For  feedback  control,  its  angular 
velocity  must  be  estimated  using  information  on  the  velocities  of  two  of  its 
points,  P\  and  P2,  under  the  assumption  that  the  ball  rolls  without  slipping  on 
a  horizontal,  rigid  floor.  The  radius  of  the  wheel  is  30  mm,  and  the  two  above 
points  lie  on  a  horizontal  diameter.  Now,  define  a  coordinate  frame  with  origin 
at  the  contact  point,  its  T-axis  in  the  direction  from  Pi  to  P2  and  its  Z-axis 
vertical,  as  sketched  in  Fig.  8.3.  Off-board  sensors  provide  reliable  estimates, 
in  mm/s,  of  pi  and  p2  as  displayed  below. 


"  120' 

"o' 

-60 

II 

-60 

_-60_ 

_  60  _ 

Pi  = 
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Fig.  8.3  A  ball-wheel 


(a)  Show  that  the  given  estimates  of  pi  and  p2  are  compatible. 

(b)  Find  the  angular  velocity  of  the  ball. 


Chapter  9 

Geometry  of  General  Serial  Robots 


9.1  Introduction 

Current  serial  robots,  encountered  not  only  in  research  laboratories  but  also  in 
production  or  construction  environments,  include  features  that  deserve  a  chapter 
apart.  We  will  call  here  general  serial  robots  all  non-redundant  serial  robots  that  do 
not  fall  in  the  category  of  those  studied  in  Chap.  4.  Thus,  the  chapter  is  devoted  to 
manipulators  of  the  serial  type  that  do  not  allow  a  decoupling  of  the  positioning  and 
the  orientation  problems.  The  focus  of  the  chapter  is,  thus,  the  inverse  displacement 
problem  (IDP)  of  general  six-revolute  robots.  While  redundant  manipulators  of  the 
serial  type  fall  within  this  category  as  well,  we  will  leave  these  aside,  for  their 
redundancy  resolution  calls  for  a  more  specialized  background  than  what  we  have 
either  assumed  or  given  here. 

A  special  feature  of  serial  manipulators  of  the  kind  studied  here  is  that  they  can 
admit  up  to  sixteen  inverse  displacement  solutions.  Such  manipulators  are  now 
in  operation  in  industry,  an  example  of  which  is  the  TELBOT  System,  shown 
in  Fig.  9.1,  which  features  all  its  six  motors  on  its  base,  the  motion  and  force 
transmission  taking  place  via  concentric  tubes  and  bevel  gears.  This  special  feature 
allows  TELBOT  to  have  unlimited  angular  displacements  at  its  joints,  no  cables 
traveling  through  its  structure  and  no  deadload  on  its  links  by  virtue  of  the  motors 
(Walischmiller  and  Li  1996). 


9.2  The  IDP  of  General  Six-Revolute  Manipulators 

As  shown  in  Chap.  4,  the  IDP  of  six-revolute  manipulators  of  the  most  general  type 
leads  to  a  system  of  six  independent  equations  in  six  unknowns.  This  is  a  highly 
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Fig.  9.1  TheTELBOT 
System  (courtesy  of 
Walischmiller  GmbH, 
Meersburg,  Germany) 


nonlinear  algebraic  system  whose  solution  posed  a  challenge  to  kinematicians  for 
about  two  decades  and  that  was  not  considered  essentially  solved  until  the  late 
eighties.  Below  we  give  a  short  historical  account  of  this  problem. 

Pieper  (1968)  reported  what  is  probably  the  earliest  attempt  to  formulate 
the  inverse  displacement  problem  of  six-axis  serial  manipulators  in  a  univariate 
polynomial  form.  He  showed  that  decoupled  manipulators,  studied  in  Sect.  4.4,  and 
a  few  others,  allow  a  closed-form  solution  of  the  inverse  displacement  problem 
associated  with  them.  However,  apart  from  the  simple  architectures  identified  by 
Pieper,  and  others  that  have  been  identified  more  recently  (Mavroidis  and  Roth 
1992),  a  six-axis  manipulator  does  not  admit  a  closed-form  solution.  Attempts  to 
derive  the  minimal  characteristic  polynomial  for  this  manipulator  were  reported  by 
Duffy  and  Derby  (1979),  Duffy  and  Crane  (1980),  Albala  (1982),  and  Alizade  et  al. 
(1983),  who  derived  a  32nd-degree  polynomial,  but  suspected  that  this  polynomial 
was  not  minimal,  in  the  sense  that  the  manipulator  at  hand  might  not  be  able 
to  admit  up  to  32  postures  for  a  given  end-effector  (EE)  pose.  Tsai  and  Morgan 
(1985)  used  a  technique  known  as  polynomial  continuation  (Morgan  1987)  to  solve 
numerically  the  nonlinear  displacement  equations,  cast  in  the  form  of  a  system  of 
quadratic  equations.  These  researchers  found  that  no  more  than  16  solutions  were  to 
be  expected.  Briefly  stated,  polynomial  continuation  consists  basically  of  two  stages, 
namely,  reducing  first  the  given  problem  to  a  system  of  polynomial  equations;  in 
the  second  stage,  a  continuous  path,  also  known  as  a  homotopy  in  mathematics,  is 
defined  with  a  real  parameter  t  that  can  be  regarded  as  time.  The  continuous  path 
takes  the  system  of  equations  from  a  given  initial  state  to  a  final  one.  The  initial  state 
is  so  chosen  that  all  solutions  to  the  nonlinear  system  in  this  state  are  either  apparent 
or  much  easier  to  find  numerically  than  those  of  the  originally  proposed  system.  The 
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final  state  of  the  system  is  the  actual  system  to  be  solved.  The  initial  system  is  thus 
deformed  continuously  into  the  final  state  upon  varying  its  set  of  parameters,  as  t 
varies  from  0  to  1.  At  each  continuation  step,  a  set  of  initial  guesses  for  each  of 
the  solutions  already  exists,  for  it  is  simply  the  solution  to  the  previous  continuation 
step.  Moreover,  finding  the  solutions  at  the  current  continuation  step  is  done  using  a 
standard  Newton  method  (Dahlquist  and  Bjorck  1974). 

Primrose  (1986)  proved  conclusively  that  the  problem  under  discussion  admits  at 
most  16  solutions,  while  Lee  and  Liang  (1988)  showed  that  the  same  problem  leads 
to  a  16th-degree  univariate  polynomial.  Using  different  elimination  procedures,  as 
described  in  Sect.  9.3,  Li^  (1990)  and  Raghavan  and  Roth  (1990,  1993)  devised 
different  procedures  for  the  computation  of  the  coefficients  of  the  univariate 
polynomial.  While  the  inverse  displacement  problem  can  be  considered  basically 
solved,  research  on  finding  all  its  solutions  safely  and  quickly  still  continued  into 
the  nineties  (Angeles  et  al.  1993).  Below  we  describe  two  approaches  to  solving 
this  problem:  (a)  the  methods  of  Raghavan  and  Roth  (1990,  1993)  and  of  Li  (1990), 
aimed  at  reducing  the  displacement  relations  to  a  single  univariate  polynomial;  and 
(b)  the  bivariate-equation  approach,  introduced  in  (Angeles  and  Etemadi  Zanganeh 
1992). 

It  will  become  apparent,  however,  that  a  streamlined  algorithm  guaranteeing  the 
reduction  of  the  system  of  \A  fundamental  equations,  as  derived  in  Sect.  9.2.2,  to  a 
lower  number  of  equations  in  only  one  or  two  unknowns,  is  still  lacking.  A  step  in 
this  direction  is  a  method  based  on  the  concept  of  kinematic  mapping,  as  reported 
by  Husty  et  al.  (2007).  Within  their  method,  the  authors  split  the  six-revolute 
kinematic  chain  into  two  three-revolute  subchains,  which  allows  the  computation 
of  the  16  inverse-displacement  solutions  using  advanced  geometric  concepts.  Once 
these  solutions  are  available,  the  16  possible  values  of  a  joint  angle  are  known, 
the  balance  five  joint  angles  are  then  computed  by  linear-equation  solving,  as  in  the 
case  of  the  algorithms  described  here. 

9.2.1  Preliminaries 

We  start  by  recalling  a  few  definitions  that  were  introduced  in  Chap.  4.  In  Sect.  4.2 
we  defined  the  matrices  Q/  and  the  vectors  associated  with  the  coordinate 
transformations  from  frame  to  frame  or,  equivalently,  the  displacement  of 
the  latter  to  the  former.  The  4x4  homogeneous  matrix — see  Sect.  2.5 — transforming 
coordinates  in  to  coordinates  in  is  given  by 


^N.B.  Lee  and  Li  of  the  references  in  this  chapter  are  one  and  the  same  person,  namely, 
Dr.-Ing.  Hongyou  Lee  (a.k.a.  Dr.-Ing.  Hongyou  Li). 
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where  0  is  the  three-dimensional  zero  vector,  while  the  3x3  rotation  matrix  Q/  and 
the  three-dimensional  vector  a/  were  defined  in  Chap.  4  as 


-XiSi 

l^iSi 

ai  Ci 

Si 

XfCi 

,  a/  = 

aiSi 

(9.2) 

_0 

A;  _ 

_  b,  _ 

In  the  above  definitions  we  used  the  Denavit-Hartenberg  notation,  whereby  at  is  the 
distance — and  hence,  at  >  0 — between  the  Z/-  and  the  Z^+i-axes,  while  bt  is  the 

offset - oo  <  bi  <  +00 — between  the  Xi-  and  X^+i-axes,  as  measured  along 

the  positive  direction  of  the  Z^  -axis.  Moreover, 

Ci  =  cos  Oi  ,  Si  =  sin  Oi  ,  A/  =  cos  o'/  ,  /X/  =  sin  o'/ 

where  Oi  is  the  Gh  joint  angle,  measured  from  X/  to  X/+i  in  the  positive  direction  of 
Z/ ,  and  O'/  denotes  the  twist  angle  from  Z/  to  Z/+i  in  the  positive  direction  of  X/+i, 
for  /  =  1, . . . ,  6.  Furthermore,  the  factoring  of  matrix  Q/ ,  introduced  in  Eq.  (4.2a), 
is  reproduced  below  for  quick  reference: 


Q.  =  Z,X.  (9.3) 

with  X/  and  Z/  denoting  two  pure  reflections,  namely. 


1  0  0 

1 

o 
_ 1 

X,-  = 

1 

o  o 

1 

1 _ ' 

z,-  = 

Si  -Ci  0 

_0  0  1_ 

(9.4a) 

Xj  =  Xi  =  X-1  Zf  =  Zi  =  Z-'  (9.4b) 

the  foregoing  reflections  thus  being  both  symmetric  and  self-inverse — see  Sect.  2.4. 
As  a  consequence. 


We  will  also  use  the  partitionings  of  Q/  displayed  in  Eq.  (4.12),  namely. 


Qi  =  [pi  q,  Ui] 


m 


T- 


n 

o 


I 

T 

i 

T 


(9.5) 


A  quick  comparison  between  Eqs.  (9.2)  and  (9.5)  leads  to  the  relations  below: 


1 

1 _ 

1 

1 _ 

1 

o 

1 _ 

3 

II 
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1 

,  n/  = 

A  /  G 
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II 

1 

■ 

_ 1 

1 

1 

i_ 

.A,-. 

(9.6) 
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T 

Further,  let  us  recall  the  definition  introduced  in  Eq.  (4.13),  e  =  [O  0  l]  ,  along 
with  that  in  Eq.  (4.3d),  a,  =  Q,  b, ,  which  readily  leads  to  =  Qf  .  Hence, 


l^iSi 

ai 

«i  =  QiC  = 

l^i  G 

_  A,-  _ 

and  b;  = 

1 

_ 1 

(9.7) 

where  we  have  reproduced  Eq.  (4.3e)  for  quick  reference.  Moreover,  since  e,  = 
[e/],  =  [e,+i]/+i,  the  above  expression  for  u/  leads  to 

Uz  =  Qz[e,+i],+i  =  [e,+i],-  (9.8a) 

which  means  that  u/  represents  e^+i  in  J^i .  Likewise, 

o,'  =  Qf  [e,]/  =  [e,],+i  (9.8b) 

Now,  using  Eqs.  (9.4a)  and  the  second  of  Eq.  (9.7),  we  introduce  the  definitions 

Yi  =  Z,a,  =  X,b,  =  [ui  0  bif  (9.9) 

whence 


(9.10) 


Furthermore,  vector  x/  of  Eq.  (4. 1 1)  is  reproduced  below  for  quick  reference  as  well: 


Xz 


COS  Oi 

sin  Oi 


(9.11) 


A  useful  concept  in  this  context  is  that  of  bilinear  form:  An  algebraic  expression 
of  the  form  Auv,  where  u  and  v  are  two  given  scalar  variables  and  A  is  independent 
of  u  and  v,  is  said  to  be  bilinear  in  u  and  v.  Likewise,  an  expression  of  the  form 
Au^v'^  is  said  to  be  biquadratic  in  u  and  v,  with  similar  definitions  for  bicubic, 
trilinear,  and  multilinear  forms.  Moreover,  the  same  definitions  apply  to  vector  and 
matrix  expressions,  as  pertaining  to  their  components  and,  correspondingly,  their 
scalar  entries. 

In  light  of  the  definition  of  X/ ,  additionally,  we  shall  refer  to  an  expression  of 
the  form 


El  =  A  cos  Oi  +  B  sin  Oi  +  C  (9.12) 

in  which  coefficients  A,  B  and  C  are  independent  of  Oi,  as  being  linear  in  x/. 
Likewise,  an  expression  of  the  form 

E2  =  A  cos  Oi  cos  Oj  -\-B  cos  Oi  sin  Oj  +C  sin  Oi  cos  Oj-\-D  sin  Oi  sin  Oj-\-E  (9.13) 
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with  coefficients  A,  B, F  independent  of  both  Of  and  Oj ,  will  be  termed  bilinear 
in  X/  and  xy .  In  fact,  such  an  expression  may  also  involve  terms  linear  in  x,  and 
xy  alone.  More  generally,  an  expression  involving  terms  with  products  such  as 
cos^  Oi  cos^  Oj  and  other  terms  with  similar  products  of  the  same  or  lower  degree 
will  be  termed  biquadratic  in  x,  and  xy .  Now  we  have 

Lemma  9.2.1.  Let  matrix  A  be  skew -symmetric  and  B  be  defined  as  the  similarity 
transformation  of  A  given  below: 


B  =  Q,AQf  (9.14) 

where  was  recalled  in  Eq.  (9.2)  and  A  is  assumed  to  be  independent  of  Oi.  Then, 
B  is  linear  in  Xf . 

Proof.  This  result  follows  from  relation  (2.139).  Indeed,  as  the  reader  can  readily 
verify,  B  is  skew- symmetric,  and  the  product  Bv,  for  any  three-dimensional  vector 
V,  can  be  expressed  in  terms  of  b,  defined  as  vect(B) — see  Sect.  2.3.3.  That  is, 

Bv  =  b  X  V 

If  a  denotes  vect(A),  then  a  and  b,  by  virtue  of  Eq.  (9.14)  and  the  results  of  Sect.  2.6, 
obey  the  relation 


b  =  Q,a 


Hence, 


Bv  =  (Q,a)  X  V 

thereby  showing  that  the  resulting  product  is  linear  in  x/ ,  q.e.d. 
Moreover,  let 


Xi  =  tan  (9.15a) 

which  allows  us  to  write  the  identities  below,  as  suggested  by  Li  (1990): 

Si  -  tiCi  =  ti,  XiSi  +  G  =  1  (9.15b) 

We  now  define  p  as  the  vector  directed  from  the  origin  of  to  the  operation  point 
(OP)  P  of  Fig.  9.2.  Moreover,  we  let  1  =  [l^,  ly,  4]^,  m  =  [m^,  m^]^,  and 

n  =  [nx,  ny,  n^Y  represent  the  three  mutually  perpendicular  unit  vectors  parallel 
to  the  X7,  T7  and  Z7  axes,  respectively,  of  2^7,  which  has  its  origin  at  P — a  layout  of 
these  axes  is  depicted  in  Fig.  4.3  for  a  decoupled  manipulator.  Hence,  the  pose  of  the 
EE  is  described  in  the  base  frame  by  means  of  the  homogeneous  transformation 
A  given  as 


Q  P 

0^  1 


Q  =  [1  m  n] 


^x 

ly  my  ny 

4 


A  = 
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Fig.  9.2  Partitioning  of  the  manipulator  loop  into  two  subloops 


In  the  next  step,  we  derive  a  set  of  scalar  equations  in  five  unknowns,  upon 
eliminating  one  of  these,  that  is  fundamental  in  computing  the  solution  of  the 
problem  at  hand. 


9.2.2  Derivation  of  the  Fundamental  Closure  Equations 

Given  the  geometric  parameters  of  the  manipulator  and  the  pose  of  the  EE  with 
respect  to  the  base  frame,  we  derive  the  manipulator  displacement  equations,  a.k.a. 
the  loop-closure  equations,  from  which  all  unknown  angles  are  to  be  computed. 
We  start  by  recalling  the  (matrix)  rotation  and  (vector)  translation  equations  of 
the  general  six-axis  manipulator,  as  displayed  in  Eqs.  (4.9a  and  b),  and  reproduced 
below  for  quick  reference: 


Q1Q2Q3Q4Q5Q6  —  Q  (9.16a) 
+  Qia2  +  QiQ2a3  +  . . .  +  QiQ2Q3Q4Q5a6  =  p  (9.16b) 

The  use  of  4  x  4  homogeneous  transformations  in  the  ensuing  preparatory  work 
will  ease  the  suitable  recasting  of  the  foregoing  equations.  Thus,  by  using  the 
matrices  A,  of  Eq.  (9.1)  in  the  above  rotation  and  translation  equations,  we  end 
up  with  a  4  X  4  matrix  equation,  namely. 


A1A2A3A4A5A6  —  A 


(9.17) 
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The  unknown  variables  in  the  above  equations  are  the  joint  angles  the 

IDP  thus  consists  in  solving  the  closure  equations  (9.16a  and  b)  or,  equivalently, 
Eq.  (9.17),  for  these  unknowns.  The  said  equations  comprise  12  scalar  equations 
and  four  identities;  however,  among  these  equations,  only  six  are  independent,  for 
the  columns  (or  the  rows)  of  a  rotation  matrix  must  form  an  orthonormal — mutually 
orthogonal  and  of  unit  magnitude — set  of  vectors.  The  orthonormality  property  of 
the  columns  or  rows  of  a  rotation  matrix,  thus,  brings  about  six  scalar  constraints. 

The  basic  approach  to  solving  the  IDP  resorts  to  disassembling  the  kinematic 
chain  of  the  manipulator  at  two  joints,  e.g.,  joints  3  and  6,  to  obtain  two  subchains  or 
subloops  (Li  et  al.  1991).  The  first  subchain,  as  suggested  in  the  foregoing  reference, 
and  depicted  in  Fig.  9.2,  goes  from  joint  3  to  joint  6  via  joints  4  and  5,  while 
the  second  subchain  goes  from  joint  6  to  joint  3  via  the  EE  and  joints  1  and  2. 
Algebraically,  this  is  equivalent  to  rewriting  Eq.  (9.17)  in  the  form 

A3A4A5  =  A^^A^^AA^^  (9.18a) 


Note  that  each  side  of  Eq.  (9.18a)  bears  a  specific  structure.  Indeed,  if  we  denote  by 
and  the  left-  and  right-hand  sides  of  Eq.  (9.18a),  we  have 


^11  ^4,  O5)  ^12(^3^  ^4,  O5)  ^13(^3^  ^4,  O5)  ^14(^3^  ^4,  O5) 

^21  (^3,  O4,  O5)  O4,  O5)  123(03,  O4,  O5)  /24(^3,  O4,  O5) 

^31(^4,  ^5)  ^32(^4,  ^5)  ^33(^4,  ^5)  /34(^4,^5) 

0  0  0  1 


(9.18b) 


ni(^i,  O2,  Oe)  ri2(0i,  O2,  Oe)  ^13(^1,  O2)  ru(0i,  O2) 
i"2i(0i,  O2,  0^)  r22(0i,  O2,  0^)  r23(0i,  O2)  ^24(^1,  O2) 
i^3i(0i,  O2,  0^)  r32(0i,  O2,  0^)  r33(0i,02)  ^34(^1,02) 

0  0  0  1 


(9.18c) 


where  Itj  and  Vij  denote  nontrivial  components  of  the  left-  and  the  right-hand 
sides,  respectively,  of  Eq.  (9.18a).  Note  that,  because  of  the  forms  of  matrices  Q;  , 
whose  third  rows  are  independent  of  6i ,  the  third  row  of  ,  as  made  apparent  in 
Eq.  (9.18b),  is  free  of  O3.  Likewise,  the  third  and  fourth  columns  of  R^,  as  made 
apparent  in  Eq.  (9.18c),  are  free  of  0^. 

It  should  be  apparent  that  other  pairs  of  joints  can  be  used  to  disassemble 
the  kinematic  chain  of  the  manipulator  into  two  subchains;  what  matters  is  that 
none  of  the  two  subchains  contains  more  than  three  joints;  else,  the  entries  of  the 
homogeneous  matrices  become  unnecessarily  complex  on  one  side  of  the  matrix 
equation,  while  the  entries  of  the  other  side  become  unnecessarily  simple. 

Now  we  extract  one  rotation  and  one  translation  equation  from  the  4x4  matrix 
equation  (9.18a),  namely, 

Q3Q4Q5  =  QrQ[QQ[  (9.19a) 

Q3(b3  +  Q4b4  +  Q4Q5b5)  =  QrQ[ (p  -  Qbg)  -  (b2  +  Q[bi)  (9.19b) 
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which  are  kinematically  equivalent  to  Eqs.  (9.16a  and  b),  but  algebraically  much 
simpler.  Note  that,  in  Eq.  (9.19b),  we  used  the  second  Eq.  (9.7)  to  substitute  a/  by 
Q;  b; .  In  the  sequel,  we  will  need  the  two  products  below: 

=  Q5[e6]6  =  (9.20a) 

Qe  e  =  Qe  [eele  =  oe  =  [ee]?  (9.20b) 

where  we  have  recalled  relations  (9.8a  and  b);  whence, 

QQe^  =  Q[e6]7  =  [cell  =  0-6  (9.20c) 

Eurther,  we  equate  the  product  of  each  of  the  two  sides  of  Eq.  (9.19a)  by  e  from  the 
right,  to  obtain,  in  light  of  Eqs.  (9.20a  and  c), 

Q3Q4[e6]5  =Q[Q[[e6]i  (9.21a) 

Both  sides  of  Eq.  (9.21a)  thus  represent  the  unit  vector  05  in  frame  T3 ;  the  difference 
between  the  two  sides  should  be  apparent:  while  the  left-hand  side  is  obtained  by 
transforming  [05]  5  into  [05]  3,  the  right-hand  side  by  transforming  [05]!  likewise.  On 
the  other  hand,  Eq.  (9.19b)  can  be  cast  in  the  form 

Q3(b3  +  Q4b4  +  Q4Q5b5)  =  -  (bz  +  Q[bi)  (9.21b) 

where  p  =  p  —  Qb6  =  [p  —  a6]i.  Hence,  the  left-  and  the  right-hand  sides  of 
Eq.  (9.21b)  represent  vector  a3  +  a4  +  as  in  frame  7^3,  the  difference  being  that  the 
left-hand  side  is  obtained  by  carrying  the  77^ -representation  of  the  vector  into  7^3, 
while  the  right-hand  side  does  so  from  the  7^i -representation  of  the  same  vector. 

Eurther,  let  the  left-  and  the  right-hand  sides  of  Eq.  (9.21a)  be  denoted  by  h  and 
i,  respectively,  while  the  counterparts  of  Eq.  (9.21b)  by  f  and  g,  i.e., 

h^h(03,^4,05)=Q3Q4U5  (9.22a) 

i  =  i(0i,02)  =  Q[Q[<T6  (9.22b) 

f  =  f(03, 0^,  65)  =  Q3(b3  +  Q4b4  +  Q4Q5b5)  (9.22c) 

g  =  ^^2)  =  Q[Q[/)  -  (b2  +  Q^bi)  =  Q[(Q[p-  bi)  -  b2  (9.22d) 

Eurther,  notice  that  arrays  f  and  g  represent,  in  fact,  the  first  three  entries  of  the 
fourth  columns  of  the  matrices  of  Eqs.  (9.18b)  and  (9.18c),  respectively.  Likewise, 
arrays  h  and  i  represent  the  third  columns  of  the  same  matrices.  Vectors  g  and  i  are 
thus  free  of  0^. 
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Now,  the  six  scalar  equations  (9.21b)  and  (9.21a)  reduce,  correspondingly,  to 


^4,  Os) 

gx(0l,  O2) 

II 

CTQ 

O 

fyiOs,  O4,  Os) 

— 

gy  (0l ,  O2) 

_  /z(^4,  Os)  _ 

.gz(0l,02) 

hxiOs,  O4,  Os) 

lx  (0l ,  O2) 

h  =  i  or 

hyiOs,  O4,  Os) 

— 

iy(0l,  O2) 

_  hz(04,0s)  _ 

Jz(0l,  ^2). 

(9.23a) 


(9.23b) 


It  should  be  noted  that  h  and  i  are  both  unit  vectors.  Thus,  each  side  of  Eq.  (9.23b) 
is  subjected  to  a  quadratic  constraint,  i.e., 

hh  =  1,  ii  =  1 

and  hence,  out  of  the  above  six  scalar  equations,  only  five  are  independent.  However, 
the  number  of  unknowns  in  these  six  equations  is  also  five.  Therefore,  Eqs.  (9.23a) 
and  (9.23b)  suffice  to  determine  the  five  unknown  joint  angles  contained  therein. 

Although  we  already  have  one  redundant  equation  to  compute  the  six  unknown 
angles,  it  will  prove  convenient  to  derive  eight  additional  equations  with  the  same 


power  products^  as  f,  g,  h  and  i,  namely, 

f.f  =g.g  (9.23c) 

f-h  =  g-i  (9.23d) 

f  X  h  =  g  X  i  (9.23e) 

(f .  f)h  -  2(f .  h)f  =  (g  .  g)i  -  2(g  .  i)g  (9.23f) 


It  is  noteworthy  that  Eq.  (9.23f)  is  derived  by  first  equating  the  reflection^  of  vector 
h  onto  a  plane  normal  to  f  with  its  counterpart,  the  reflection  of  vector  i  onto  a  plane 
normal  to  g.  The  final  form  of  Eq.  (9.23f)  is  obtained  upon  clearing  denominators  in 
the  foregoing  reflection  equation. 

Equations  (9.23a-9.23f)  amount  to  14  scalar  equations  in  five  unknown  joint 
variables  {0i}\.  These  are  \hQ  fundamental  closure  equations  sought.  Some  facts 
pertaining  to  the  degree  of  the  two  sides  of  Eqs.  (9.23c-f)  are  proven  below: 

Fact  9.2.1.  The  inner  products  f-fand  f -h  are  both  free  ofx^  and  bilinear  in  {  x/  }\, 
while  their  counterparts  g  •  g  and  g  •  i  are  bilinear  in  x\  and  X2. 


^By  power  product  we  mean  terms  with  their  coefficients  deleted;  for  example,  the  power  products 
of  the  polynomial  5x^y  +  3xz  +  9y^  +  4z  =  0  are  the  terms  x^y,  xz,  y^  and  z. 

^Neither  Li  nor  Raghavan  and  Roth  disclosed  the  geometric  interpretation  of  this  fourth  equation, 
first  proposed  by  Lee  and  Liang  (1988). 
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Proof. 


ff  = 


QsCbs  +  04^4  +  Q4Q5b5)|P 
=  11^3  +  04^4  +  Q4Q5b5|P 


=  Y,  II b,-  f  +  2bjQ4(b4  +  Qsbs)  +  2b[Q5bs 


whose  rightmost-hand  side  is  clearly  free  of  X3  and  is  bilinear  in  {  x/  Similarly, 


f  •  h  =  (b3  +  Q4b4  +  Q4Q5b5)^QrQ3Q4U5 


=  b[  Q4U5  +  b4  U5  +  b5  Q5  U5 


J 


whose  rightmost-hand  side  is  apparently  bilinear  in  X4  and  X5,  except  for  the  last 
term,  which  contains  two  factors  that  are  linear  in  X5,  and  hence,  can  be  suspected 
to  be  quadratic.  However,  Qsbs  is,  in  fact,  as,  while  U5  is  the  last  column  of  Qs,  the 
suspicious  term  thus  reducing  to  a  constant,  namely,  bsco^as.  Similar  proofs  for 
g  •  g  and  g  •  i  will  be  given  presently.  Moreover,  □ 

Fact  9.2.2.  Vector  fxh  is  trilinear  in  { x,  }^,  while  its  counterpart,  g  x i,  is  bilinear 
in  { x;  }\. 

Proof  If  we  want  the  cross  product  of  two  vectors  in  frame  A  but  have  these  vectors 
in  frame  B,  then  we  can  proceed  in  two  ways:  either  (a)  transform  each  of  the  two 
vectors  into  ^-coordinates  and  perform  the  cross  product  of  the  two  transformed 
vectors;  or  (b)  perform  the  product  of  the  two  vectors  in  S-coordinates  and  then 
transform  the  product  vector  into  ^-coordinates.  Obviously,  the  two  products  will 
be  the  same,  which  allows  us  to  write 


f  X  h  =  Qs  [b3  X  (Q4U5)  +  (Q4b4)  x  (Q4U5)  +  (Q4Q5b5)  x  (Q4U5)] 

=  Qsibs  X  (Q4U5)  +  Q4(b4  X  U5)  +  Q4  [(Qsbs)  x  U5)]} 

whose  rightmost-hand  side  is  apparently  trilinear  in  {x,  jj,  except  for  the  term  in 
brackets,  which  looks  quadratic  in  X5.  A  quick  calculation,  however,  reveals  that  this 
term  is,  in  fact,  linear  in  X5  as  well.  Indeed,  from  the  definitions  given  in  Eqs.  (4.3c 
and  d)  and  (9.5)  we  have 


(Qsbs)  x  U5  =  as  x  U5  = 


asXsSs  +  bsnscs 
-flsAsCs  -I-  bsixsss 
-asixs 


which  is  obviously  linear  in  X5.  The  proof  for  the  counterpart  product,  gxi,  parallels 
the  foregoing  proof,  and  will  be  given  below.  □ 

Fact  9.2.3.  Vector  (f  •  f)h  —  2(f  •  h)f  is  trilinear  in  {  x,  }^,  its  counterpart,  (g  •  g)i  — 
2(g  •  i)g,  being  bilinear  in  {  X/  }\. 
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Proof.  First,  we  write  the  (elongated  or  contracted)  reflection  of  vector  h  in  the  form 

(ff)h-2(fh)f  =  Q3V 


where 


V  = 


Q4U5  -  2[(u5  Q4b3)b3  +  (U5  b4)b3  +  (Uj  b4)Q4b 


+  (u[ Q5b5)b3  +  (U5  Q5b5)Q4b4  +  (U5  Q5b5)Q4Q5b5]  +  2w 


J 


Q4U5  -  2[U5  b4(b3  +  b3  +  Q4b4) 


+  U5  Q5b5(b3  +  Q4b4  +  Q4Q5b5)  +  2w 


with  all  terms  on  the  right-hand  side,  except  for  w,  which  will  be  defined  presently, 
clearly  bilinear  in  X4  and  X5.  Vector  w  is  defined  as 


W  =  [  ]l  +  [  ]2  +  [  Is 


each  of  the  foregoing  brackets  being  expanded  below: 

[  ]l  =  [(b^Q4b4)Q4U5  -  (U5Q[b3)Q4b4] 

=  Q4(u5b[Q[  -b4U5Q[)b3 
=  Q4(U5b[  -b4U5)Q[b3 

which  thus  reduces  to  a  product  including  a  factor  of  the  form  Q/AQf ,  with  A 
being  the  term  in  parentheses  in  the  rightmost-hand  side  of  the  last  equation.  This  is 
obviously  a  skew-symmetric  matrix,  and  Lemma  9.2.1  applies,  i.e.,  the  rightmost- 
hand  side  of  the  last  equation  is  linear  in  X4.  This  term  is,  hence,  bilinear  in  X4 
and  X5.  Furthermore, 

[  h  =  [(bjQ5b5)Q4U5  -  (U5b4)Q4Q5b5] 

=  Q4  [(bj  Q[b4)u5  -  (u[b4)Q5b5] 

=  Q4(U5b5  Q[  -  Q5b5U5)b4 

which  is  apparently  linear  in  X4,  but  it  is  not  obvious  that  it  is  also  linear  in  X5. 
To  show  that  the  second  linearity  also  holds,  we  can  proceed  in  two  ways.  First, 
note  that  the  term  in  parentheses  is  the  skew-symmetric  matrix  Usa^  —  asu^ , 
whose  vector,  as  x  U5,  was  already  proven  to  be  linear  in  X5.  Since  the  vector 
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of  a  skew- symmetric  matrix  fully  defines  that  matrix — see  Sect.  2.3 — the  linearity 
of  the  foregoing  term  in  X5  follows  immediately.  Alternatively,  we  can  expand  the 
aforementioned  difference,  thereby  deriving 

0  (25/X5  -asXsCs  +  bsii5S5~ 

-asfis  0  -asXsSs  -  bsiisCs 

^sXsCs  —  bsfisSs  asXsSs  +  bsfisCs  0 

which  is  clearly  linear  in  X5.  Moreover,  its  vector  can  be  readily  identified  as  as  x  U5, 
as  calculated  above.  Finally, 

[  h  =  [(15^04 Q5b5)Q4U5  -  (U5Q[b3)Q4Q5b5] 

=  Q4(U5b[Q5  -  Q5b5U5)Q4b3 
=  Q4(u5a5  -  ajUj  )Q4  b3 

this  bracket  thus  reducing  to  a  product  including  the  factor  Q/AQf,  with  A 
skew- symmetric.  Hence,  the  foregoing  expression  is  linear  in  X4,  according  to 
Lemma  9.2.1.  Moreover,  the  matrix  in  parentheses  was  already  proven  to  be  linear 
in  X5,  thereby  completing  the  proof  for  vector  (f  •  f)h  —  2(f  •  h)f.  The  proof  for  vector 
(g  •  g)i  ~  parallels  the  foregoing  proof  and  will  be  given  presently.  □ 

Finally,  we  have  one  more  useful  result: 

Fact  9.2.4.  If  a  scalar,  vector,  or  matrix  equation  is  linear  in  x^,  then  upon 
substitution  of  Ci  and  Si  by  their  equivalent  forms  in  terms  of  Xi  =  tan(^//2),  the 
foregoing  equation  becomes  quadratic  in  Xt  after  clearing  denominators. 

Proof  We  shall  show  that  this  result  holds  for  a  scalar  equation,  with  the  extension 
to  vector  and  matrix  equations  following  directly.  The  scalar  equation  under 
discussion  takes  the  general  form 


T  T 

U5a5  -a5U5  = 


Aci  “h  Psi  C  —  0 


where  the  coefficients  A,  B,  and  C  do  not  contain  Oi .  Upon  substituting  Ci  and  Si  in 
terms  of  r,  =  tan  (0^  /2),  and  multiplying  both  sides  of  that  equation  by  1  +  x^,  we 
obtain 


A(1  —  x^^  +  2Bxi  +  C(1  +  x^^  —  0 
which  is  clearly  quadratic  in  r/ ,  q.e.d. 

Moreover,  if  a  scalar,  vector,  or  matrix  equation  is  of  degree  k  in  x/,  upon 
introducing  the  same  trigonometric  substitution,  the  said  equation  becomes  of 
degree  2k  in  xi . 
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Expressions  for  the  right-hand  sides  of  Eqs.  (9.23c-d)  are  given  below: 

2 


g  g  = 


2/Qi(Q2b2+bi)  +  2b[Q2b2 


1 

g  i  =  (/)  -  QiQ2b2  -  Qibi) 

g  X  i  =  Q[Q[(p  X  (Te)  -b2  X  Q^Q^ae  -  Q[(bi  x  Q[(r6) 


(9.24a) 

(9.24b) 

(9.24c) 


and 


(g  •  g)i 


2(g  •  i)g 


-2[((T[/))(Q[Q[/)-b2-Q[bi)  +  (a[QiQ2b2)b2 

+(o-[Qibi)b2  +  ((Te  bi]  +  2w' 


(9.24d) 


In  deriving  and  simplifying  the  above  relations,  we  use  the  invariance  relations — see 
Sect.  2.7 — of  the  dot  and  cross  products,  i.e.,  for  any  arbitrary  vectors  u  and  v,  we 
have 


(Q/u)^(Q,v)  =  u^v 
(Qiu)  X  (Qiv)  =  Q,  (u  X  v) 

All  the  terms  on  the  right-hand  sides  of  Eqs.  (9.24a-d),  except  for  w',  are  apparently 
bilinear  in  xi  and  X2.  This  bilinearity  also  holds  for  the  last  term  in  Eq.  (9.24d),  i.e., 
w',  which  can  be  expressed  in  the  form 

w'^[  ];  +  [  ]^  +  [  I'a  (9.25) 

Each  of  the  above  brackets  is  given  as 

[  ]'i  ^  [(o-rQiQ2b2)QrQ[/o  -  (/o^QiQ2b2)QrQ[o-6] 

=  -  ‘T6/0^)(Q2Qi)b2  (9.26a) 

[  ]'  ^  [(b[Q2b2)Q[Q[o-6  -  (o-6^QiQ2b2)Q[bi] 

=  Qr[(Q[‘^6)bi  -bi(Q[(T6)^]Q2b2  (9.26b) 

[  ];  ^  [(o-[Qibi)Q[Q[/)  -  (/Qibi)Q[Q[a6] 

=  QI [Q[ ipol  -  <^6/>^)Qi]bi  (9.26c) 

According  to  Lemma  9.2.1,  the  terms  in  the  right-hand  sides  of  relations  (9.26a-c) 
are  all  bilinear  in  xi  and  X2. 
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It  is  noteworthy  that  the  third  components  of  vectors  f  x  h  and  (f  •  f)h  —  2(f  •  h)f, 
as  well  as  f  •  f  and  f  •  h,  are  all  free  of  O3.  Hence,  among  the  14  scalar  equations,  i.e., 
Eqs.  (9.23a-f),  six  are  free  of  O3.  Casting  all  14  equations  in  vector  form  results  in 
\hQ  fundamental  closure  equations: 


PX45  =  RXi2 


(9.27) 


where  P  and  R  are  14x9  and  14x8  matrices,  respectively.  Moreover,  the  entries  of 
P  are  linear  in  X3,  while  those  of  R  are  independent  of  the  joint  angles.  In  addition, 
the  nine-  and  eight-dimensional  vectors  X45  and  x  12  are  defined  as 


X45 


[^4^5 


^4^5  C4^5  C4C5  ^4  C4  ^5  C5 


(9.28a) 


X12 


[^1^2  S1C2  C1S2  C1C2  Si  Cl  S2  C2] 


T 


(9.28b) 


Various  approaches  have  been  reported  to  solve  the  fundamental  closure  equa¬ 
tions  for  the  unknown  joint  angles,  but  all  methods  fall  into  two  categories:  (a) 
purely  numerical  approaches,  whereby  no  attempt  is  made  to  reduce  the  number 
of  unknowns  (Angeles  1985),  or  the  reduction  is  rather  limited,  from  six  to  four 
unknowns  (Tsai  and  Morgan  1985);  and  (b)  elimination  approaches,  whereby 
unknowns  are  eliminated  algebraically,  as  opposed  to  numerically,  until  a  reduced 
number  of  equations  in  a  reduced  number  of  unknowns  is  derived. 

We  focus  here  only  on  the  second  category.  Of  these,  we  have  essentially 
two  classes:  (a)  the  univariate-polynomial  approach  and  (b)  the  bivariate -equation 
approach.  As  the  names  indicate,  the  former  aims  at  reducing  the  fundamental 
equations  to  one  single  equation  in  one  unknown.  Moreover,  that  single  equation, 
being  polynomial  in  form,  is  termed  the  characteristic  polynomial  of  the  problem  at 
hand.  The  polynomial  is  derived  upon  substituting  the  cosine  and  sine  functions  of 
the  unknown  angle,  say  Ox,hy  (I  —  T^) /(I  +  T^)  and  27/(1  +  T^),  respectively,  with 
T  =  tan(^x/2).  This  transformation  is  well  known  as  the  tan-half  trigonometric 
identities.  The  second  approach,  in  turn,  aims  at  reducing  all  fundamental  closure 
equations  to  a  smaller  system  of  trigonometric,  as  opposed  to  polynomial,  equations 
in  only  two  unknowns. 

The  transformation  of  the  original  problem  given  in  terms  of  trigonometric 
functions  of  the  unknown  angles  into  a  polynomial  equation  in  T  is  essential  from  a 
conceptual  viewpoint,  for  this  transformation  makes  apparent  that  the  problem  under 
study  admits  a  finite  number  of  solutions,  namely,  the  degree  of  the  characteristic 
polynomial.  On  the  other  hand,  the  same  transformation  is  not  trouble-free.  Indeed, 
the  mapping  from  0^  into  T  apparently  includes  a  singularity  at  whereby 

T  ^00.  The  outcome  is  that,  if  one  of  the  solutions  is  Ox  =  n,  then  the 
characteristic  polynomial  admits  at  least  one  solution  at  infinity,  which  is  reflected 
in  a  deflation  of  the  polynomial.  This  phenomenon,  called  polynomial  deflation,  was 
made  apparent  in  Example  4.4.3,  where  a  quartic  characteristic  polynomial  appeared 
as  cubic  because  of  one  solution  at  infinity.  The  beginner  may  thus  be  misled  to 
believing  that,  in  the  presence  of  a  solution  at  infinity,  the  system  at  hand  admits  a 
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smaller  number  of  solutions  than  it  actually  does.  Furthermore,  in  the  neighborhood 
of  Ox  =  n,  one  of  the  solutions  is  extremely  large  in  absolute  value,  which  thus 
gives  rise  to  numerical  inaccuracies,  generically  referred  to  as  ill-conditioning.  As  a 
matter  of  fact,  the  problem  of  polynomial-root  finding  has  been  identified  as  ill- 
conditioned  by  numerical  analysts  for  some  time  (Forsythe  1970). 

In  order  to  cope  with  the  foregoing  shortcomings  of  the  tan-half  identities,  the 
author  and  his  team  devised  an  alternative  means,  the  bivariate-equation  approach, 
to  solving  the  problem  at  hand  and  other  similar  ones  in  computational  kinematics 
(Angeles  and  Etemadi  Zanganeh  1992a,b).  In  this  approach,  the  14  equations  are 
reduced  to  a  system  of  bivariate  trigonometric  equations  in  the  sines  and  cosines  of 
two  of  the  unknown  angles.  These  equations  are  then  plotted  in  the  plane  of  the  two 
unknowns,  thus  obtaining  four  contours,  whose  intersections  yield  the  real  values 
of  the  two  unknowns.  As  a  matter  of  fact,  only  two  such  equations  would  suffice; 
however,  it  turns  out  that  the  underlying  reduction  cannot  be  accomplished  without 
the  introduction  of  either  extra  equations  or  spurious  roots,  which  must  be  detected 
in  order  to  discard  them.  Notice  that,  for  an  intersection  point  to  qualify  as  a  solution, 
all  contours  must  meet  at  that  point.  As  illustrated  with  one  example,  even  the  use 
of  extra  contours  does  not  guarantee  a  legitimate  solution.  Spurious  solutions  fail  to 
allow  for  the  computation  of  the  remaining  four  joint  angles. 


9.3  The  Univariate-Polynomial  Approach 

We  describe  here  two  procedures  leading  to  one  single  univariate  16th-degree 
polynomial  equation,  which  is  the  characteristic  polynomial  of  the  system  at  hand. 
The  two  procedures  bear  many  similarities,  but  they  also  involve  remarkable 
differences  that  warrant  separate  discussions. 


9.3.1  The  Raghavan-Roth  Procedure 


A  sophisticated  elimination  procedure  was  proposed  by  Raghavan  and  Roth  (1990, 
1993).  Their  procedure  is  based  on  Eqs.  (9.23a-f),  but  their  14  closure  equations  are 
different,  as  explained  below. 

At  the  outset,  Raghavan  and  Roth  define  four  vectors  that  will  play  a  key  role  in 
the  ensuing  derivations,  namely. 


f  ^f(04,^5)  = 


7i 
/2 
.h. 


=  Zjf  =  X3  (b3  +  Q4b4  +  Q4Q5b5)  (9.29a) 


hx 
hi 

V.hij 


h  =  h(0i)  = 


Q2g  +  a2  =  Q[/t)  -  bi 


(9.29b) 


9.3  The  Univariate-Polynomial  Approaeh 


391 


Table  9.1  Expressions  for  the  eomponents  of  veetors  f,  h,  r,  and  n 


Item 

Expression 

Item 

Expression 

/i 

C4O  “h  S^i2  ~\~  CI3 

ri 

C4mi  +  ^4^2 

/2 

— A3(^4ti  —  C^i2) 

ri 

—A3  (^4^1  —  C4m2) 

+ 

+  ^3^3 

/s 

r3 

P3(S4mi  —  C4m2) 

+  ^3^3  +  bz 

+  X3m3 

C5CL5  CLa 

mi 

S5P5 

U 

m2 

C5A4/X5  +  P4X5 

^3 

ssPacis  +  X^bs  b^ 

m3 

— C5/X4/X5  +  A4A5 

hi 

c\p  +  siq  —  a\ 

ni 

Ciu  + 

h2 

-Xi{sip  -ciq) 

n2 

—Xi(siu  —  civ) 

+  pi{r  -bi) 

+  PlW 

hi 

pi{sip-ciq) 

n3 

Pl(SiU  —  CiV) 

+  Ai(r  —  b\) 

“h  Ai w 

P 

-Ixae  -  {mxPe  +  nxXe)be 
+  Px 

u 

mxPe  +  nxXe 

q 

—lyGe  —  (rriyPe  +  qnyXe)be 
+  Py 

V 

myPe  3“  nyX^ 

r 

—l^ae  —  {rriziie  +  nzXe)be 
+  Pz 

w 

m^pe  +  n^Xe 

r  =  f  (04,  05)  = 


n 

ri 


—  Z3h  —  X3Q4U5 


=  Q2i  =  Q[o'6 


(9.29c) 


(9.29d) 


Expressions  for  the  components  of  the  above  four  vectors  are  given  in  Table  9.1, 
where  {i  =  1,2,3),  p,  q,  r,  u,  v,  and  w  are  auxiliary  variables.  Using 
Eqs.  (9.29a-d),  (9.3),  (9.4b),  (9.10),  and  (9.9),  we  can  rewrite  Eqs.  (9.21a  and  b) 
in  terms  of  the  foregoing  vectors,  namely, 


Z3f(04,05)  =  X2Z2n(0i)  (9.30a) 

Z3f(04,  05)  =  X2[Z2h(0i)  -  ^2]  (9.30b) 

where  we  have  recalled  definitions  (9.9)  for  i  =  2.  These  six  scalar  equations  play  a 
key  role  in  deriving  the  Raghavan-Roth  equations  in  five  unknowns  that  are  needed 
to  solve  the  problem  at  hand. 

Next,  both  sides  of  Eqs.  (9.30a  and  b)  are  multiplied  from  the  left  by  = 
X2  =  X2;  then,  the  two  equations  thus  resulting  are  rearranged  in  the  forms 

X2Z3f  y2  =  Z2h 
X2Z3r  =  Z2n 


(9.31) 

(9.32) 
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Now,  four  new  vectors,  the  counterparts  of  those  introduced  in 
defined  as 

Eq.  (9.22a-d),  are 

f  =  X2Z3f  y 2  —  ^2(^3?  +  b2) 

(9.33a) 

g  =  Z2h 

(9.33b) 

h  =  X2Z3f 

(9.33c) 

i  =  Z2n 

(9.33d) 

Note  that  f  and  h  are  trilinear^  in  X3,  X4,  and  X5,  while  the  first  two  components 
of  g  and  1  are  bilinear  in  xi  and  X2,  their  third  components  being  linear  in  xi  and 
free  of  O2.  Similar  to  Eqs.  (9.23a  and  b),  six  scalar  equations  are  obtained: 


f  =  g  (9.34a) 

h  =  1  (9.34b) 

Moreover,  eight  more  scalar  equations  are  obtained  in  the  forms 

f.f  =g.g  (9.34c) 

f-h  =  g-i  (9.34d) 

f  X  h  =  g  X  I  (9.34e) 

(f  •  f)h  -  2(f  •  h)f  =  (g  •  g)i  -  2(g  •  i)g  (9.34f) 


The  fourteen  scalar  equations  (9.34a-f)  are  henceforth  termed  the  Raghavan-Roth 
(RR)  equations. 

The  third  components  of  the  two  vectors  on  the  right-hand  sides  of  Eqs.  (9.34e 
and  f),  and  the  terms  on  the  right-hand  sides  of  Eqs.  (9.34c  and  d)  are  free  of  O2 
and  linear  in  xi .  As  proven  by  Raghavan  and  Roth  in  the  above  references,  the  eight 
foregoing  equations  have  the  same  power  products  as  f,  h,  g,  and  1.  Now,  the  14  RR 
equations  (9.34a-f)  are  cast  in  the  form 

PX45  =  Rxi2  (9.35) 

where  P  and  R  are  14x9  and  14x8  matrices,  respectively.  Moreover,  the  entries 
of  P  are  linear  in  X3,  while  those  of  R  are  independent  of  the  joint  angles;  moreover, 
R  has  the  structure: 


while  the  last  row  of  Z3  is  free  of  ^3,  the  last  row  of  X2Z3  is  [/X2^3,  — /X2C3,  A2]. 
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X 
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X 

X 

0 

0 

X 

X 

0 

0 

0 

0 

X 

X 

0 

0 

(9.36) 


In  the  above  display,  all  nonzero  entries  are  denoted  by  x  and  rows  are  written 
according  to  the  order  of  appearance  in  Eqs.  (9.34a-f).  This  special  structure  of 
matrix  R  is  then  exploited  to  eliminate  the  joint  angles  0i  and  O2  in  an  efficient  way. 
Based  on  the  structure  of  R,  two  groups  of  six  and  eight  equations  are  defined: 

PmX45  =  Cxi  (9.37a) 

P/X45  =  Axi2  (9.37b) 

where  C  is  a  6  x  2  constant  matrix  that  is  formed  by  the  nonzero  entries  in  rows 
3,  6,  7,  8,  11,  and  14  of  matrix  R.  A  is,  in  turn,  an  8  x  6  matrix  whose  entries  are 
all  functions  of  the  data,  while  xi  and  X45  were  defined  in  Eqs.  (9.11)  and  (9.28a), 
respectively;  x  12  is,  in  turn,  the  six-dimensional  vector  defined  as 

X12  =  [ii52  S1C2  CxS2  C1C2  ^2  C2]  (9.38) 

Eurthermore,  P^  comprises  the  third,  sixth,  seventh,  eighth,  11th  and  14th  rows  of  P, 
P/  comprising  the  remaining  eight  rows.  Notice  that  Fu  and  P/  are  both  linear  in  X3. 

Any  two  of  the  six  scalar  equations  in  Eq.  (9.37a)  can  now  be  used  to  solve  for 
Xi,  the  resulting  expression  then  being  substituted  into  the  remaining  four  equations 
of  the  same  group.  This  is  done  by  first  partitioning  the  six  scalar  equations  as 

C^xi  =  (9.39a) 

C/Xi  =  d/  (9.39b) 

where  and  C/  are  2  x  2  and  4x2  submatrices  of  C,  respectively,  with  d^  and  d/ 

being  the  corresponding  two-  and  four-dimensional  vectors  that  result  from  PmX45; 
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these  two  vectors  are  trilinear  in  X3,  X4  and  X5.  If  Eq.  (9.39a)  is  solved  for  xi  and 
the  result  is  substituted  into  Eq.  (9.39b),  we  obtain  four  equations  free  of  0i  and  O2, 
namely, 


r4X45  =  -  d,  =  O4  ,  r4  =  C;C;‘(P„)2  -  (P„)4  (9.40a) 

in  which  F  4  is  a  4  x  9  matrix  whose  entries  are  linear  in  X3,  while  (Pm)2  and  (Pm)4 
are  2  X  9  and  4x9  submatrices  of  matrix  P^^,  respectively.  The  above  set  of  equations 
is  now  cast  in  the  form 


I>iy3  =  O4  (9.40b) 

with  Di  defined  as  a  4  x  3  matrix  whose  entries  are  bilinear  in  X4  and  X5,  while  O4 
is  the  four-dimensional  zero  vector,  and  y3  is  defined  as 

y3  =  [C3  ^3  1]^  (9.41) 

If  Cu  is  chosen  with  nonzero  entries  in  the  third  and  sixth  rows  of  matrix  R,  then 
we  have 


flip  -fiiq 
ll\U  —piiv 


(9.42a) 


with  p,  q,  u,  and  v  listed  in  Table  9.1.  If  is  nonsingular,  ^  is  readily 
obtained  as 


1 

piiiuq  —  pv) 


—V  q 
—u  p 


(9.42b) 


However,  if  turns  out  to  be  singular,  then  a  different  pair  of  Eqs.  (9.37a),  of  the 
set  associated  with  rows  3,  6,  7,  8,  11  and  14,  should  be  selected. 

Additional  equations  free  of  0i  and  O2  can  be  derived  from  any  six  of  the 
eight  equations  in  Eq.  (9.37b).  Indeed,  these  six  equations  is  all  that  is  needed  to 
solve  for  X12  in  terms  of  ^3,  0^  and  the  expressions  thus  resulting  would  then 
be  substituted  into  the  remaining  two  equations  of  the  same  set,  to  obtain  two 
additional  equations  free  of  0i  and  O2.  However,  this  elimination  process  is  not 
suitable  for  symbolic  computations.  Instead,  Raghavan  and  Roth  (1990)  derived  the 
two  additional  equations  in  a  terser  form.  This  is  done  by  finding  two  independent 
linear  combinations  of  the  eight  equations  (9.37b)  that  render  identically  zero  all 
terms  in  0i  and  O2.  The  left-hand  sides  of  these  equations  are  given  as 

01(03,04,05)  =  +  —SiJ. 

2ai  2ai  ai 

X  h);t  +  ju-iwfy  -  -bi)hy  (9.43a) 
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02(^3,  O4,  O5) 


2  2 

-ixxwf^  +  iii{r  -bi)h^  +  —Sify  -  (9.43b) 

ai  ^  2ai 


while  the  right-hand  sides  are 


p-[(S-g)ix-X(g-i)gJ  -  p-8ii^  +  —S2g^ 

2ai  2ai  ai 

-Xxjxiig  X  i);t  +  l^iwgy  -  li\{r  -bx)iy 


(9.43c) 


lit  _ _  _ _  __ 

fi  =  ^[(g-g)*y-2(g-i)gj,]-Ai/ii(gxi)3, 

2  2 

-jiiwg^  +  ^ll{r  -  bi)T^  +  —hgy  -  ^hJy  (9.43d) 

a\  ^  2a\ 

On  the  other  hand,  hx,Tx,  f  x  ~Sx  represent  the  first  components  of  vectors  h, 
I,  f,  and  g,  respectively,  the  other  components  being  defined  likewise.  Furthermore, 
and  62  are  defined  as 


51  =  +  (r  -  biY  -  a\ 

52  =  pu  -\-  qv  -\-  {r  —  bi)w 

Upon  substitution  of  g  and  I,  as  given  by  Eqs.  (9.33b  and  d),  respectively,  into 
Eqs.  (9.43c  and  d),  and  introduction  of  the  definitions  given  in  Table  9.1,  it  turns 
out  that  both  and  \lf2  vanish  identically,  i.e., 

ijri  =  0  and  \[f2  =  0 

Also  note  that,  in  deriving  expressions  (9.43a  and  b)  and  (9.43c  and  d),  we 
assume  that  ai  ^  0.  However,  ai  vanishes  in  many  industrial  robots,  those  having 
their  first  two  axes  intersecting — usually  at  right  angles — the  foregoing  procedure 
thus  becoming  inapplicable.  One  way  of  coping  with  this  case  is  to  go  one  step 
behind  Raghavan  and  Roth’s  procedure  and  redefine,  fork  =  1,2, 

^4,  O5)  ^ ^4,  ^5); 


and 
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i.e., 

M03,0,,ds)  ^ 

— (3iAi/xi(f  X  h);c  +  aijjiiw f  y  —  ai/Jiiir  —  bi)hy  (9.44a) 
(piiOs,  04,65)  =  -  2.(( -h)  f  ^]  -  xh)y  -aiix^wf^ 

+  aiixiir  -bi)hx  +  1^82 f  y  -  ^8jiy  (9.44b) 

2  2 

fi  =  Y[(g  •  S)tx  -  2(g  • 

-aiXiHiig  X  I);t  +  ainiwgy  -  aiHi(r  -  bi)Ty  (9.45a) 

u2 

=  Y[(g-g)*y  -2(g-\)gy]-aiXiixi(gx\)y 

_  ^2  _ 

-ainiwg^  +  ainiir  -  bi)ix  +  lj\82gy  -  (9.45b) 

Under  their  new  definitions,  apparently,  \lri  and  \[f2  also  vanish.  Once  and  02  are 
equated  to  zero,  two  equations  are  obtained  that  can  be  cast  in  the  form 

r  2X45  =  ^2  (9.46) 


or  equivalently. 


D2y3  =  O2  (9.47) 

where  O2  is  the  two-dimensional  zero  vector,  F  2  is  a  2  x  9  matrix  whose  entries  are 
linear  in  X3,  D2  is  a  2  x  3  matrix  whose  entries  are  bilinear  in  X4  and  X5,  and  y3  was 
introduced  in  Eq.  (9.41). 

The  two  Eqs.  (9.40a)  and  (9.46)  thus  involve  a  total  of  six  scalar  equations  free 
of  0i  and  O2,  and  can  be  combined  to  yield  a  system  of  six  equations  trilinear  in  X3, 
X4,  and  X5,  namely. 


i:x45  =  06  (9.48a) 

where  E  is  a  6  x  9  matrix  whose  entries  are  linear  in  X3,  and  06  is  the  six¬ 
dimensional  zero  vector.  Now,  the  tan-half  trigonometric  identities  relating  Si  and  C/ 
with  Ti  =  tan(0//2),  for  i  =4,5,  are  substituted  into  Eq.  (9.48a).  Upon  multiplying 
the  two  sides  of  the  equation  thus  resulting  by  (1  +  r|)(l  +  r|),  Raghavan  and  Roth 
obtained 


E  'X45  =  06 


(9.48b) 
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where  E'  is  a  6  x  9  matrix  that  is  linear  in  X3,  while  X43  is  defined  as 

X45  =  [t|t|  T4T5  t|  T4t|  T4T5  T4  t|  T5  1]^ 

If  the  same  trigonometric  identities,  for  i  =  3,  are  now  substituted  into 
Eq.  (9.48b),  and  then  the  first  four  scalar  equations  of  this  set  are  multiplied  by 
(1  +  r|)  to  clear  denominators,  the  equation  thus  resulting  takes  the  form 

=  (9.48c) 

In  the  above  equations,  E ''  is  a  6  x  9  matrix  whose  first  four  rows  are  quadratic 
in  r3,  while  its  last  two  rows  are  apparently  rational  functions  of  13.  However, 
as  reported  by  Raghavan  and  Roth,  the  determinant  of  any  6x6  submatrix  of 
is,  in  fact,  an  8th-degree  polynomial  in  13  and  not  a  rational  function  of  the 
same.  Moreover,  in  order  to  eliminate  14  and  rs,  they  resort  to  dialytic  elimination 
(Salmon  1964),  introduced  in  this  book  in  Sect.  5.4.1  and  in  Exercise  5.11.  Dialytic 
elimination  is  further  discussed  in  Sect.  9.3,  in  connection  with  the  Li,  Woernle,  and 
Hiller  method,  and  in  Sect.  10.2  in  connection  with  parallel  manipulators. 

In  applying  dialytic  elimination,  the  two  sides  of  the  system  of  equations 
appearing  in  Eq.  (9.48c)  are  first  multiplied  by  then,  the  system  of  equations 
thus  obtained  is  adjoined  to  the  original  system,  thereby  deriving  a  system  of  12 
linear  homogeneous  equations  in  X45,  namely, 

Sx45  =  O12  (9.48d) 

where  O12  is  the  12-dimensional  zero  vector,  while  the  12-dimensional  vector  X45  is 
defined  as 


X45  =  [ 


3  2 

^4^5 


2  2 
^4^5 


-r2-r 

^4^5 


r4r|  1415  r4  r|  rs  1  ]  ^ 
Eurthermore,  the  12x12  matrix  S  is  defined  as 


its  6  X  12  blocks  G  and  K  taking  on  the  forms 

g^[e"  063],  k^[o63  5:"] 

with  063  defined  as  the  6x3  zero  matrix. 


(9.48e) 
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Now,  in  order  for  Eq.  (9.48d)  to  admit  a  nontrivial  solution,  the  determinant  of 
its  coefficient  matrix  must  vanish,  i.e., 

det(S)  =  0  (9.49) 

which  is  the  characteristic  equation  sought.  The  foregoing  determinant  turns  out  to 
be  a  16th-degree  polynomial  in  Moreover,  the  roots  of  this  polynomial  give  the 
values  of  corresponding  to  the  16  solutions  of  the  IDP.  It  should  be  noted  that, 
using  the  same  procedure,  one  can  also  derive  this  polynomial  in  terms  of  either 
T4  or  ts  if  the  associated  vector  in  Eq.  (9.48d)  is  written  as  X35  or  X34,  respectively. 
Consequently,  the  entries  of  matrix  E  would  be  linear  in  either  X4  or  X5. 


9.3.2  The  Li-Woernle-Hiller  Procedure 


At  the  outset,  the  factoring  of  Q/  given  in  Eq.  (4.1c)  and  the  identities  first  used  by 
Li  (1990),  namely,  Eqs.  (9.15b),  are  recalled.  Additionally,  Li  defines  a  matrix  T/  as 


Hence, 


-Ti  1  0 

1  T  0 
0  0  1 


Ti  1  0 

1  -Ti  0 
0  0  1 


with  C,  defined  in  Eq.  (4.1b).  Eurthermore,  we  note  that  the  left-hand  sides  of  the 
four  vector  equations  (9.23a,  b,  e  and  f)  are  of  the  form  Q3V,  where  v  is  a  three- 
dimensional  vector  independent  of  0^ .  Upon  multiplication  of  the  above-mentioned 
equations  from  the  left  by  matrix  T3,  Li  and  co-authors  obtained  a  new  set  of 
equations,  namely. 


U3f  =  T3g 
U3f  =  T3i 

U3(fxf)  =  T3(gxi) 


(f  •  f)f  -  2(f  •  h)f  =  T3  [(g  •  g)i  -  2(g  •  i)g] 


(9.50a) 

(9.50b) 

(9.50c) 

(9.50d) 
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where  f  and  f  are  defined  as 


f  ^  A3(b3  +  Q4b4  +  Q4Q5b5)  (9.51) 

f  ^  A3(Q4U5)  (9.52) 


with  Ai  defined,  in  turn,  in  Eq.  (4.1c). 

Because  of  the  form  of  matrices  T3  and  U3,  the  third  of  each  of  the  four  vector 
equations  (9.50a-d)  is  identical  to  its  counterpart  appearing  in  Eqs.  (9.34a,  b,  e 
and  f).  That  is,  if  we  denote  by  either  Vf  or  (v)/  the  ith  component  of  any  three- 
dimensional  vector  V,  the  unchanged  equations  are 


/V 

h 

yv 

n 

(f  X  f)3 
(f-f)r3-2(f.h)/3 


^3 

h 

(g  X  1)3 

(g  •  g)h  -  2(g  •  i)g3 


(9.53a) 

(9.53b) 

(9.53c) 

(9.53d) 


all  of  which  are  free  of  6^ .  Eurthermore,  six  additional  equations  linear  in  13  will  be 
derived  by  multiplying  both  sides  of  Eqs.  (9.53a-d)  and  of  (9.23c  and  d)  by  13,  i.e.. 


r3[(f-f)r3  - 


^3/3 

=  r3g3 

(9.54a) 

^3^3 

=  T^3h 

(9.54b) 

r3(f  X  f)3 

=  r3(gxi)3 

(9.54c) 

2(f  •  h)/3]3 

=  T3[(g-g);3  -2(g-i)g3]3 

(9.54d) 

r3(f-f) 

=  ^■3(g-g) 

(9.54e) 

r3(f-h) 

=  T3(g-i) 

(9.54f) 

We  now  have  20  scalar  equations  that  are  linear  in  13,  namely,  the  12 
Eqs.  (9.50a-d)  plus  the  six  equations  (9.54a-f)  and  the  two  scalar  equations  (9.23c 
and  d).  Moreover,  the  left-hand  sides  of  the  foregoing  20  equations  are  trilinear  in 
r3,  X4,  and  X5,  while  their  right-hand  sides  are  trilinear  in  13,  xi,  and  X2.  These  20 
equations  can  thus  be  written  in  the  form 


Ax  =  P  (9.55a) 

where  the  20  x  1 6  matrix  A  is  a  function  of  the  data  only,  while  the  20-dimensional 
vector  P  is  trilinear  in  13,  xi,  and  X2,  the  16-dimensional  vector  x  being  defined,  in 
turn,  as 


X  =  [T3C4C5  T3C43'5  T3i4C5  T33'43'5  T3C4  r33'4  T3C5  XsSs 

C4C5  C4S5  S4C5  S4S5  C4  S4  C5 


(9.55b) 
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Next,  matrix  A  and  vector  P  are  partitioned  as 


At/ 

Al 


(9.56) 


where  Au  is  a  nonsingular  16  x  16  matrix,  is  a  4  x  16  matrix,  vector  Pjj 
is  16-dimensional,  and  vector  is  4-dimensionaL  Moreover,  the  two  foregoing 
matrices  are  functions  of  the  data  only.  Thus,  we  can  solve  for  x  from  the  first  16 
equations  of  Eq.  (9.55a)  in  the  form 

X  = 

Upon  substituting  the  foregoing  value  of  x  into  the  four  remaining  equations  of 
Eq.  (9.55a),  we  derive  four  equations  free  of  x,  namely, 

(9.57) 

In  Eq.  (9.57)  the  two  matrices  involved  are  functions  of  the  data  only,  while  the 
two  vectors  are  trilinear  in  13,  xi,  and  X2.  These  equations  are  now  cast  in  the  form 

{AiC2  +  BiS2  +  Ci)x2  +  DiC2  +  EiS2  Fj  =  0,  i  =  1, 2,  3, 4  (9.58a) 

where  all  coefficients  At, ,  Ff  are  linear  in  xi.  Next,  Li  and  co-authors  substitute 
C2  and  ^2  in  the  foregoing  equations  by  their  equivalents  in  terms  of  r2  =  tan  (^2/ 2), 
thereby  obtaining,  for  /  =  1,  2,  3,  4, 


+  ^^1X2X2  +  AiiX2  +  FiiX2  +  2FiX2  +  Da  —  0  (9.58b) 

with  the  definitions 

An  =  Af  +  Ci  (9.58c) 

Ca  ^  Ci  -  Ai  (9.58d) 

Da  =  Di  +  Fi  (9.58e) 

Fa  ^  Fi  -  Di  (9.58f) 

Eurther,  X2  and  13  are  both  eliminated  dialytically  from  the  four  equations  (9.58a). 
To  this  end,  both  sides  of  all  four  equations  (9.58b)  are  multiplied  by  r2,  which 
yields 


Cax^x^  +  25/r|r3  +  A;/r2t3  +  Fax^  +  2FiX2  +  DiiX2  —  0 


(9.58g) 


We  have  now  eight  equations  that  are  linear  homogeneous  in  the  eight¬ 
dimensional  nonzero  vector  z  defined  as 


z  =  [r|r3  r|r3  r|  X2X2  x^  X2  l] 


T 


(9.58h) 
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and  hence,  the  foregoing  eight-dimensional  system  of  equations  takes  the  form 


Mz  =  0 


(9.59) 


where  the  8x8  matrix  M  is  simply 


0 

Cii 

0 

2Bi 

Fn 

^11 

2Ei 

Dn 

0 

C22 

0 

2B2 

F22 
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F33 

^33 

2E3 
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F>33 
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C44 

2B4 

F44 

A  44 

2E4 
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D44 

0 

Now,  since  z  is  necessarily  nonzero,  Eq.  (9.59)  should  admit  nontrivial  solutions, 
and  hence,  matrix  M  should  be  singular,  which  leads  to  the  condition  below: 

det(M)  =  0  (9.60) 

Thus,  considering  that  all  entries  of  M  are  linear  in  xi,  det(M)  is  octic  in  xi,  and 
hence,  Eq.  (9.60)  is  equally  octic  in  Xi.  By  virtue  of  Eact  9.2.2,  then,  Eq.  (9.60)  is  of 
16th  degree  in  ri ;  this  equation  takes  the  form 

16 

y]a^Tf=0  (9.61) 

0 

which  is  the  characteristic  equation  sought,  its  roots  providing  up  to  16  real  values 
of  0i  for  the  IDP  at  hand. 


9.4  The  Bivariate-Equation  Approach 

The  difference  between  this  approach  and  those  leading  to  the  univariate  polyno¬ 
mial,  as  outlined  in  Sect.  9.3,  lies  in  three  aspects:  (a)  only  four,  out  of  the  six 
original  unknowns,  are  eliminated;  (b)  the  tan-half  identities  are  avoided,  in  order  to 
avoid  polynomial  deflation  at  or  around  values  of  tt,  and  to  allow  for  finding  all  real 
roots;  and  (c)  direct  polynomial-root  finding  is  avoided,  rough  estimates  of  all  roots 
being  found,  first,  by  inspection,  then  refined  by  means  of  a  Newton  procedure. 

Now,  to  derive  the  bivariate  equations,  we  have  to  eliminate  three  of  the  five 
unknowns  from  the  14  fundamental  closure  equations.  To  this  end,  we  resort 
to  Eqs.  (9.40b),  which  are  trilinear  in  {x/}^.  Eurthermore,  from  definition  (9.41), 
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y3  7^  0,  and  hence,  the  4x3  matrix  Di  of  Eq.  (9.40b)  must  be  rank-deficient, 
which  means  that  every  one  of  its  four — the  number  of  combinations  of  four  objects 
taking  three  at  a  time — 3  x  3  determinants,  obtained  by  deleting  one  of  its  four 
rows,  should  vanish.  We  need,  in  principle,  only  two  of  these  determinants  to  obtain 
two  independent  equations  in  O4  and  65.  To  be  on  the  safe  side  regarding  spurious 
roots  and  formulation  singularities,^  we  impose  the  vanishing  of  all  four  possible 
determinants,  which  yields,  correspondingly,  four  contours  in  the  64-65  plane;  the 
intersections  of  all  contours  then  yield  the  real  (64,  65)  pairs  of  values  which  render 
Di  rank-deficient.  Each  of  the  four  equations  thus  derived  describes  a  contour  C,, 
for  /  =  1 , 2,  3, 4,  in  the  64-65  plane: 

C,  :  Fi(64,65)  =  0,  /  =  1,2,3,4.  (9.62) 

Note  that,  by  plotting  the  four  contours  in  a  square  of  the  64-65  plane,  of  side  2n, 
we  ensure  that  no  real  solutions  will  be  missed. 

The  intersection  points  can  be  detected  visually  by  the  user  or,  automatically, 
by  a  suitable  graphical  user  interface  (GUI).^  Regardless  of  the  detection  method, 
numerical  code  can  be  employed  to  refine  each  pair  (64, 65)  of  intersection 
coordinates  to  the  desired  accuracy.  The  well-known  Newton-Raphson  method 
for  nonlinear-equation  solving,  outlined  in  Sect.  B. 3,  can  be  used  here.  However, 
this  method  works  for  solving  systems  of  as  many  equations  as  unknowns.  In  our 
case,  we  end  up  with  four  nonlinear  equations  in  only  two  unknowns.  While,  in 
principle,  any  two  of  those  four  equations  can  be  used  to  solve  for  the  two  unknowns, 
numerical  roundoff  error  and  the  numerical  conditioning  of  the  problem  at  hand, 
to  be  discussed  in  Sect.  9.4.1,  will  invariably  lead  to  different  numerical  solutions 
for  different  choices  of  those  two  equations.  The  question  then  is  which  of  the 
four  distinct  solutions  to  pick  up.  In  order  to  avoid  this  quandary,  we  suggest  here 
to  regard  all  four  equations  as  independent,  entailing  possible  contradictions — 
roundoff  errors  may  render  the  four  equations  independent,  which  they  aren’t. 
With  this  approach,  then,  rather  than  one  solution  to  the  four  equations,  what 
we  seek  is  their  least-square  approximation,  which  can  be  done  using  a  method 
known  as  Newton-Gauss  (Dahlquist  and  Bjorck  1974),  as  outlined  in  Sect.  B. 4. 
Alternatively,  Matlab’s  function  Isqnonlin  can  be  used  to  find  the  same  least- 
square  approximation.  In  any  event,  the  problem  is  solved  iteratively.  Within  the 
Newton-Gauss  method,  a  linear  overdetermined  system  of  equations  is  solved  at 
each  iteration,  using  one  of  the  methods  of  Sect.  B.l. 


^Formulation  singularities  occur  when,  in  the  absence  of  a  kinematic  singularity — characterized 
by  the  vanishing  of  det(J),  for  J  defined  as  in  Eq.  (5.10b) — two  or  three  contours  Ci  are  tangent  at 
an  intersection.  When  this  is  the  case,  and  a  pair  of  functions  (9.62)  is  chosen  to  find  their  roots, 
whose  contours  are  tangent,  the  numerical  computation  of  the  coordinates  of  the  intersection  point 
becomes  impossible. 

^The  intersection  points  appearing  in  Figs.  9.3  and  9.4  were  obtained  using  the  Matlab  GUI 
developed  by  Dr.  Stephane  Caro,  a  postdoctoral  fellow  at  McGill  University’s  Robotic  Mechanical 
Systems  Laboratory.  The  GUI  is  available  in  the  CD  accompanying  this  edition 
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In  this  way,  two  of  the  unknown  joint  angles,  O4  and  O5,  are  computed  accurately, 
the  remaining  four  unknowns  being  determined  uniquely,  as  described  in  Sect.  9.6. 
Notice,  however,  that  spurious  solutions  to  the  IDP  are  likely  to  occur.  These 
are  intersections  of  the  four  contours  which,  although  verifying  the  four  equa¬ 
tions  (9.62),  fail  to  produce  a  full  set  of  solutions  {  Of  }^.  The  computation  of  all 
remaining  joint  variables,  Oi,  O2,  O3  and  0^,  once  O4  and  O5  are  available,  is  the 
subject  of  Sect.  9.6.3. 


9.4.1  Numerical  Conditioning  of  the  Solutions 

We  recall  here  the  concept  of  condition  number  of  a  square  matrix  (Golub  and  Van 
Loan  1989),  as  introduced  in  Sect.  5.8.  In  this  subsection  we  stress  the  relevance 
of  the  concept  in  connection  with  the  accuracy  of  the  computed  solutions  of  the 
general  IDP. 

The  concept  of  condition  number  of  a  square  matrix  is  of  the  utmost  importance 
because  it  measures  the  roundoff-error  amplification  upon  solving  a  system  of  linear 
equations  having  that  matrix  as  coefficient.  The  condition  number  of  a  matrix, 
discussed  in  Sect.  5.8,  can  be  computed  in  many  possible  ways.  For  the  purpose 
at  hand,  it  will  prove  convenient  to  work  with  the  condition  number  defined  in  terms 
of  the  Frobenius  norm,  as  given  in  Eqs.  (5.80a  and  b). 

In  the  context  of  the  bivariate-equation  approach,  we  can  intuitively  argue  that 
the  accuracy  in  the  computation  of  a  solution  is  dictated  by  the  angle  at  which  two 
contours  giving  a  solution  intersect.  Thus,  the  solutions  computed  most  accurately 
are  those  determined  by  contours  intersecting  at  right  angles.  On  the  contrary,  the 
solutions  computed  least  accurately  are  those  obtained  by  tangent  contours.  We  shall 
formalize  this  observation  in  the  discussion  below. 

We  distinguish  between  the  condition  number  of  a  matrix  and  the  conditioning  of 
a  solution  of  a  nonlinear  system  of  equations.  We  define  the  latter  as  the  condition 
number  of  the  Jacobian  matrix  of  the  system,  evaluated  at  that  particular  solution. 
More  concretely,  let 


fl(XuX2)  =  0 
/2(Xi,X2)  =  0 


be  a  system  of  two  nonlinear  equations  in  the  two  unknowns  Xi  and  X2.  Moreover, 
the  Jacobian  matrix  of  this  system  is  defined  as 


F  = 


L(V/2)^, 


(9.63) 


where  V  /<;  denotes  the  gradient  of  function  fk{xi,  X2),  defined  in  turn  as 


dfk/dxi 

dfkldx2_ 


(9.64) 
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It  is  to  be  noted  that  multiplying  each  of  the  two  given  equations  by  a  scalar 
other  than  zero  does  not  affect  its  solutions,  each  Jacobian  row  being,  then, 
correspondingly  multiplied  by  the  same  scaling  factor.  To  ease  matters,  we  will 
assume  henceforth  that  each  of  the  above  equations  has  been  properly  scaled  so 
as  to  render  its  gradient  a  unit  vector  in  the  plane  of  the  two  unknowns.  In  order 
to  calculate  the  condition  number  of  F,  which  determines  the  conditioning  of  the 

'T' 

solutions,  we  calculate  first  FF  and  its  inverse,  namely. 


and 


1 

v/l  •  v/2' 

1  cos  y 

.V/l  •  V/2 

1 

cos  y  1 

1 


sin^  y 


1 

—  cos  y 


—  cos  y 
1 


where  y  is  the  angle  at  which  the  contours  intersect.  The  condition  number  Kf  of  F, 
based  on  the  Frobenius  norm,  can  then  be  computed  as 


Kf  = 


1 

sin  yl 


—  7t  <  y  <  7t 


(9.65) 


which  means  that  for  the  best  possible  solutions  from  the  numerical  conditioning 
viewpoint,  the  two  contours  cross  each  other  at  right  angles,  whereas  at  singular 
configurations,  the  contours  are  tangent  to  each  other.  The  reader  may  have 
experienced  that,  when  solving  a  system  of  two  linear  equations  in  two  unknowns 
with  the  aid  of  drafting  instruments,^  the  solution  becomes  fuzzier  as  the  two  lines 
representing  those  equations  become  closer  to  parallel. 


9.5  Implementation  of  the  Solution  Method 

Whatever  method  is  chosen  to  solve  the  IDP,  the  solution  procedure  will  eventually 
require  numerical  computations.  Indeed,  both  the  univariate-polynomial  and  the 
bivariate-equation  approaches  ultimately  resort  to  a  numerical  procedure  to  find 
either  the  roots  of  a  polynomial  equation  that  can  be  of  up  to  16th  degree 
or,  correspondingly,  the  solutions  of  a  system  of  trigonometric  equations.  Now, 
formulas  for  the  roots  of  polynomial  equations  are  available  only  for  the  quadratic, 
the  cubic  and  the  quartic  polynomials^ ;  those  for  the  cubic  and  quartic  equations  are 


^Graphical  methods  of  mechanism  analysis  rely  on  this  form  of  linear-equation  solving. 

^The  Italian  mathematicians  Niccolo  Tartaglia — meaning  the  “stammerer,”  his  real  name  believed 
to  have  been  Fontana — (1535)  and  Girolamo  Cardano  (1545),  independently,  or  so  each  claimed, 
found  the  formula  for  the  three  roots  of  the  cubic  equation,  now  known  as  Cardan’s  formula. 
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so  cumbersome  that  in  practice  they  are  seldom  applied.  The  Italian  mathematician 
Ruffini  gave  a  sketch  of  a  proof  in  1799  showing  that  formulas  for  the  roots  of 
polynomials  of  fifth  or  higher  degree  are  not  possible  in  general  (Wells  1986).  Then, 
the  Norwegian  mathematician  Abel,  in  1826,  provided  a  more  rigorous  proof  of 
the  same  result.  It  was  the  genius  of  the  French  Evariste  Galois  (1811-1832)  that, 
aided  by  Galois’  own  theory  of  groups  (Livio  2005),  led  to  an  elegant  theory  on  the 
solvability  of  polynomial  equations  that  closed  an  important  chapter  in  the  history 
of  mathematics. 

Now,  when  numerically  solving  the  equations  involved,  whether  polynomial  or 
trigonometric,  intermediate  computations  can  yield  coefficients  with  absolute  values 
of  disparate  orders  of  magnitude,  which  is  prone  to  numerical  instabilities — ill- 
conditioning.  These  occur  naturally  in  the  neighborhood  of  singularities,  and  cannot 
be  avoided.  Another  source  of  ill-conditioning  lies  in  the  data  themselves.  When 
working  with  two  different  sets  of  equations,  one  representing  point  displacements, 
the  other  angular  displacements,  we  end  up  with  a  mixture  of  equations  with 
physical  units  of  length  and  equations  that  are  dimensionless.  Such  a  mixture  is 
a  source  of  ill-conditioning,  which  can  be  avoided  without  too  much  effort:  we 
recommend  to  start  by  rendering  the  point-displacement  equations  dimensionless, 
which  can  be  done  by  dividing  the  DH  parameters  { <2/ ,  bt  introduced  in  Sect.  4.2 
and  the  position  vector  p  of  the  EE  operation  point  by  the  characteristic  length  L 
introduced  in  Sect.  5.8.  This  stage,  which  can  be  termed  normalization,  is  done  in 
the  numerical  examples  included  in  Sect.  9.7. 

Furthermore,  when  refining  the  rough  estimates  of  the  contour  intersections, 
as  occurring  in  the  implementation  of  the  bivariate-equation  approach,  we  are 
confronted  with  computing  the  least- square  approximation  to  an  overdetermined 
system  of  nonlinear  equations.  This  is  a  well-researched  problem  in  the  realm  of 
numerical  analysis  (Dahlquist  and  Bjorck  1974).  While  effective  methods  exist  that 
solve  the  problem  without  resorting  to  gradients,  we  have  used  in  the  solutions 
an  in-house  developed  package  of  C  routines  and  Matlab  functions,  ODA,  for  a 
broad  class  of  problems  occurring  in  mathematical  programming.^  In  this  library,  we 
have  a  routine,  LSSNLS,  that  implements  the  Newton-Gauss  algorithm  described 
in  Sect.  B.4.  LSSNLS  requires  an  initial  guess  xq  for  the  unknown  vector  x  as  well 
as  information  on  the  dimensions  n  of  x,  the  number  of  unknowns,  and  of  f(x),  the 
number  of  equations,  m  >  n.  Then,  LSSNLS  returns  an  optimum  value  x*  that  best 
approximates  the  overdetermined  system  of  equations  f(x)  =  0  in  the  least-square 
sense,  and  that  is  dependent  on  xq.  In  the  absence  of  ill-conditioning,  x*  is  the  local 
optimum  of  the  problem  closest  to  the  initial  guess  xq.  However,  the  Matlab  GUI  that 
was  developed  by  Dr.  Caro — see  footnote  6 — to  automate  the  refining  of  the  visual 


Ferrari’ s  formula — so  named  after  the  Italian  mathematician  Ludovico  Ferrari,  a  disciple  of 
Cardano’s — provides  the  four  roots  of  a  quartic  polynomial. 

^The  ODA  library  is  available  on  www.mcgill.ca/~/rmsl/Angeles_html/courses/MECH577/. 
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estimates  relies  on  Matlab’s  1  sqnonl  in  function.  The  method  implemented  in  this 
function  is  direct,  in  that  it  is  based  solely  on  function  evaluations,  thus  obviating 
gradient  computations. 


9.6  Computation  of  the  Remaining  Joint  Angles 

So  far  we  have  reduced  the  system  of  displacement  equations  to  either  one  single 
univariate  polynomial  in  the  tangent  of  half  one  of  the  joint  angles — the  univariate- 
polynomial  approach — or  a  system  of  bivariate  trigonometric  equations  in  the  sines 
and  cosines  of  two  joint  angles — the  bivariate-equation  approach.  In  either  case,  we 
still  need  a  procedure  to  compute  the  remaining  joint  angles,  which  is  the  subject  of 
the  balance  of  this  section. 


9.6.1  The  Raghavan-Roth  Procedure 

The  most  straightforward  means  of  computing  0/[  and  Os  in  this  procedure  is 
Eq.  (9.48d),  which  can  be  interpreted  as  an  eigenvalue  problem  associated  with 
the  12  X  12  matrix  S,  and  has  one  known  eigenvalue,  namely,  0,  for  its  sole 
variable,  O3,  was  computed  so  as  to  render  S  singular.  Now,  every  scientific  package 
offers  eigenvalue  calculations,  whereby  the  eigenvectors  are  usually  produced  in 
a  normalized  form,  i.e.,  with  all  eigenvectors  computed  as  unit  vectors.  Let,  for 
example,  a  be  the  12-dimensional  eigenvector  of  S  corresponding  to  the  zero 
eigenvalue.  In  this  case,  ||(r||  =  1,  but  X45,  the  solution  sought,  is  obviously  of 
magnitude  greater  than  unity,  for  its  12th  component,  cr^,  is  exactly  1,  according 
to  its  definition,  Eq.  (9.48e).  In  order  to  produce  X45  from  a,  then,  all  we  need 
is  a  suitable  scaling  of  this  vector  that  will  yield  (x45)i2  =  1.  We  thus  have  that 
cri2  7^  0 — otherwise,  Eqs.  (9.48d)  would  be  inconsistent — and  hence, 

1 

X45  =  — a 

cri2 

The  outcome  will  be  a  set  of  unique  values  of  O4  and  O5  for  each  of  the  16  possible 
values  of  O3 . 

Next,  0i  and  O2  are  computed  from  Eq.  (9.35),  which  is  rewritten  below  in  a  more 
suitable  form: 


Rxi2  =  X345  (9.66a) 

with  the  14-dimensional  vector  X345  defined  as 


X345  =  PX45 


(9.66b) 
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Since  R  is  a  14  x  8  matrix,  Eq.  (9.66a)  comprises  14  linear  equations  in  the 
eight  unknown  components  of  xu.  Although  any  eight  of  the  14  equations  (9.66a) 
suffice,  in  principle,  to  determine  xu,  we  should  not  forget  that  these  computations 
will  most  likely  be  performed  with  finite  precision,  and  hence,  roundoff-error 
amplification  is  bound  to  occur.  In  order  to  keep  roundoff  errors  as  low  as  possible, 
we  recommend  to  use  all  the  foregoing  14  equations  and  calculate  Xi2  as  the  least- 
square  approximation  of  the  overdetermined  system  (9.66a).  This  approximation 
will  be,  in  fact,  the  solution  of  the  given  system  because  all  14  equations  are 
compatible.  The  least-square  solution  of  this  system  yields,  symbolically, 

X12  =  (R^R)-'R^X345  (9.66c) 


In  practice,  the  foregoing  least- square  approximation  is  computed  using  an  orthog- 
onalization  procedure  (Golub  and  Van  Loan  1989),  the  explicit  or  the  numerical 
inversion  of  the  product  H^H  being  advised  against  because  of  its  frequent 
ill-conditioning.  Appendix  B  outlines  the  robust  numerical  computation  of  the 
least-square  approximation  of  an  overdetermined  system  of  equations  using  orthog- 
onalization  procedures.  The  only  remaining  unknown  is  0^,  which  is  computed 
below:  This  unknown  is  readily  computed  from  Eq.  (4.9a).  Indeed,  the  first  of  the 
three  vector  equations  represented  by  this  matrix  equation  yields 

Q1Q2Q3Q4Q5P6  =  q  (9.67a) 


where  q  denotes  the  first  column  of  Q,  while,  according  to  Eq.  (9.5),  p6  denotes  the 
first  column  of  matrix  Q6,  i.e.. 


COS  06 

”9^11" 

P6  = 

sin  06 

,  q  = 

qii 

_  0  _ 

Thus,  Eq.  (9.67a)  can  be  readily  solved  for  p6,  i.e., 

P6  =  Q[QlQrQ[Q[q 


(9.67b) 


(9.68) 


thereby  obtaining  a  unique  value  for  0^  for  every  set  of  values  of  {0k}l.  This 
completes  the  solution  of  the  IDP  under  study. 


9. 6.2  The  Li-Woernle-Hiller  Procedure 

Once  0i  is  available,  the  remaining  angles  are  computed  from  linear  equations: 
Equations  (9.59)  are  first  rearranged  in  nonhomogeneous  form,  namely. 


Nz'  =  n 


(9.69) 
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with  the  8x7  matrix  N  and  the  seven-  and  eight-dimensional  vectors  z'  and  n 
defined  as 


0 

Cii 

0 

2Bi 

Fn 

^11 

2Ei 

0 

C22 

0 

2B2 

F22 

^22 

2E2 

0 

C33 

0 

2B3 

F33 

^33 

2E3 

0 

C44 

0 

2B4 

F44 

A  44 

2E4 

Cii 

2Bx 

^11 

^11 

2Ei 

0 

Dn 

6^22 

2B2 

F22 

^22 

2E2 

0 

F>22 

C33 

2^3 

T33 

^33 

2E3 

0 

F>33 

C44 

2B4 

F44 

A  44 

2E4 

0 

D44 

r  3  ”1 

'Dn 

r|r3 

2 

D22 

X^X3 

3 

D33 

D44 

^2^3 

^2 

,  n  = 

0 

^2 

0 

^3 

0 

_  "^2  _ 

_  0  _ 

Now,  Eq.  (9.69)  represents  an  overdetermined  linear  algebraic  system  of  eight 
equations,  but  only  seven  unknowns.  Again,  we  recommend  here  a  least- square 
approach  to  cope  with  ill-conditioning.  In  this  way,  the  solution  of  Eq.  (9.69)  can 
be  expressed  symbolically  in  the  form 

z'  =  (N^N)“^N^n 

With  z'  known,  both  X2  and  ,  and  hence,  O2  and  0^ ,  are  known  uniquely.  Eurther, 
with  0\,  O2,  and  O3  known,  the  right-hand  side  of  Eq.  (9.55a),  p,  is  known.  Since  the 
coefficient  matrix  A  of  that  equation  is  independent  of  the  joint  angles,  A  is  known, 
and  that  equation  can  be  solved  for  vector  x  uniquely.  Once  x  is  known,  the  two 
angles  ^4  and  Os  are  uniquely  determined,  with  0^  the  sole  remaining  unknown; 
this  can  be  readily  determined,  also  uniquely,  as  discussed  in  connection  with  the 
Raghavan-Roth  method. 


9. 6.3  The  Bivariate -Equation  Approach 

After  all  common  intersections  of  the  four  foregoing  contours  have  been  determined, 
we  have  already  two  of  the  unknowns,  ^4  and  Os,  the  remaining  four  unknowns  being 
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calculated  uniquely  as  described  presently.  First,  we  calculate  one  of  the  remaining 
joint  variables,  O3,  using  Eq.  (9.40b).  For  this  purpose,  we  evaluate  matrix  Di  for  all 
intersection  points.  Then,  we  rewrite  the  same  equation  in  the  form 


Hx3  =  T  (9.70a) 

the  4x2  matrix  H  being  obtained  from  Di  by  excluding  its  last  column,  which 
is  denoted  by  —  t.  Moreover,  matrix  H  and  the  four-dimensional  vector  r  are  both 
bilinear  in  X4  and  X5  and  hence,  known.  Again,  we  use  all  four  equations  (9.70a) 
at  our  disposal  to  compute  the  two-dimensional  vector  X3  using  a  least-square 
approach.  If  H  is  of  full  rank — its  two  columns  are  linearly  independent — then  the 
solution  can  be  expressed  symbolically  in  the  form 

X3  =  (9.70b) 

However,  if  H  is  rank-deficient,  i.e.,  if  its  two  columns  are  linearly  dependent,  then 
the  inverse  appearing  in  Eq.  (9.70b)  cannot  be  computed,  the  solution  (^4,  Os)  being 
spurious.  In  fact,  even  if  H  is  of  full  rank,  the  computed  X3  may  fail  to  be  a  unit 
vector,  as  required  by  its  definition.  The  outcome  here  is  that 

cos^  Os  +  sin^  Os  I 

which  means  that  the  value  of  X3  computed  from  Eq.  (9.70a)  will  yield  a  complex 
value  of  Os.  In  this  case,  the  intersection  (^4,  Os)  at  stake  is  spurious  as  well. 

WhenH  is  of  full  rank  and  the  computed  X3  is  of  unit  Euclidean  norm,  Eq.  (9.70b) 
determines  Os  uniquely  for  the  given  values  of  ^4  and  Os. 

With  Os ,  ^4  and  Os  known,  we  can  now  calculate  0i  and  O2  simultaneously  from 
Eq.  (9.27),  which  we  reproduce  below  in  a  more  suitable  form 

Rxi2  =  X345  (9.71) 

where  R  is  a  14x8  matrix  depending  only  on  the  problem  data,  while  X345,  defined  as 

X345  =  PX45  (9.72) 

is  a  14-dimensional  vector  trilinear  in  X3,  X4,  and  X5,  and  is  hence,  known.  Moreover, 
matrices  P  and  R  as  well  as  vectors  X12  and  X45  were  defined  in  Eqs.  (9.27) 
and  (9.28a  and  b).  Again,  we  have  an  overdetermined  system,  of  14  equations, 
in  eight  unknowns  this  time,  which  can  best  be  solved  for  X12  using  a  least- 
square  approach  with  an  orthogonalization  procedure.  The  unique  solution  of  the 
overdetermined  system  at  hand  can  thus  be  expressed  as 


X12  =  (R^R)-‘R^X345  (9.73) 

Note  that  the  solution  thus  obtained  determines  Xi  and  X2  uniquely,  the  only 
remaining  unknown  being  0^,  which  is  computed  as  in  Eq.  (9.68). 
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9.7  Examples 


We  solve  the  examples  below  using  the  bivariate-equation  approach  with  the 
purpose  of  both  helping  the  reader  visualize  the  real  solutions  and  avoiding  the 
formulation  singularities  brought  about  by  the  tan-half  identities. 

Example  9.7.1.  Find  all  inverse-displacement  solutions  of  the  Fanuc  Arc  Mate  S 
series  manipulator  of  1990  for  the  end-effector  pose  given  below: 


"0  1  O' 

'  130' 

Q  = 

0  0  1 

,  p  = 

850 

_1  0  0_ 

_1540_ 

in  which  p  is  given  in  mm,  the  DH  parameters  of  the  robot  being  given  in  Table  5.2. 


Solution:  For  starters,  we  divide  the  DH  parameters  {at ,bi}\  and  vector  p  by 
L  =  351.23  mm,  the  characteristic  length  of  this  manipulator  found  in  Sect.  5.8. 
In  following  the  bivariate-equation  approach,  we  plot  the  four  contours  in  the  O^-Os 
plane  guaranteeing  that  matrix  Di  of  Eq.  (9.40b)  is  rank-deficient.  The  four  contours 
are  superimposed  in  Fig.  9.3,  where,  apparently,  we  can  detect  eight  intersections. 


^2,  rad 


Fig.  9.3  Contours  Ci,  C2,  C3,  and  C4  for  the  Fanuc  Arc  Mate  S  series  manipulator  of  1990 


^^The  accompanying  CD  includes  a  GUI  allowing  the  user  to  automate  the  computation  of  accurate 
values  of  the  joint  variables  by  clicking  at  the  visual  estimates  of  the  intersections  of  all  four 
contours. 
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Table  9.2  Rough  estimates  of  the  coordinates  of  the  intersection  points  of  the  Fanuc 
Arc  Mate  S  series  manipulator  of  1990 


Sol’n  no. 

1 

2 

3 

4 

5 

6 

7 

8 

O4  (rad) 

2.38 

1.57 

0.34 

-1.57 

-3.06 

-2.39 

-1.57 

1.57 

05  (rad) 

-2.97 

-1.57 

-1.79 

-1.57 

1.75 

2.97 

1.57 

1.57 

Table  9.3  Refined  estimates 
of  the  coordinates  of  the  eight 
intersection  points  of  Fig.  9.3 

Inters ’n  no. 

64  (rad) 

O5  (rad) 

1 

2.381637539 

-2.973100582 

2 

1.570796327 

-1.570796327 

3 

0.344592933 

-1.797513978 

4 

-1.570796327 

-1.570796327 

5 

-3.060229795 

1.755742649 

6 

-2.397512950 

2.972311705 

7 

-1.570796327 

1.570796327 

8 

1.570796327 

1.570796327 

Table  9.4  Legitimate  solutions  of  the  inverse  displacement  of  the 
Fanuc  Arc  Mate  S  series  manipulator  of  1990  at  the  given  pose 


Sol’n  no. 

1 

3 

5 

6 

94 

136.457° 

19.743° 

-175.338° 

-137.367° 

Os 

-170.346° 

-102.989° 

100.596° 

170.300° 

The  coordinates  (O4,  O5)  of  each  intersection  point  are  first  estimated  by  inspection, 
as  listed  in  Table  9.2.  Further,  we  submit  each  of  these  eight  values  as  an  initial  guess 
to  the  Newton-Gauss  procedure — or  Matlab’s  function  Isqnonlin — to  find  the 
least-square  approximation  of  the  overdetermined  system  of  four  equations  in  two 
unknowns  of  Eq.  (9.62).  The  eight  solutions  thus  found  are  then  used  to  compute  X3 
of  Eq.  (9.70a).  As  it  turned  out,  solutions  2,  4,  7  and  8  led  to  a  rank-deficient  H,  and 
were,  thus,  discarded  as  spurious.  For  the  record,  we  include  all  eight  least-square 
solutions  found  in  radians,  in  Table  9.3. 

The  legitimate  solutions  are  displayed  in  Table  9.4,  in  degrees  for  easier 
visualization.  The  robot  thus  admits  four  real  inverse  displacement  solutions  at  the 
given  pose. 

The  values  of  the  remaining  angles  are  recorded  in  Table  9.5. 

Example  9.72.  Here  we  include  an  example  of  a  manipulator  admitting  16  real 
inverse  displacement  solutions.  This  manipulator  was  proposed  by  Li  (1990),  its 
Denavit-Hartenberg  parameters  appearing  in  Table  9.6. 

Solution:  The  foregoing  procedure  was  applied  to  this  manipulator  for  an  end- 
effector  pose  given  as 
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Table  9.5  Remaining  angles 
corresponding  to  the 
solutions  of  Table  9.4 


Table  9.6  DH  parameters  of 
Li’s  manipulator 


Sol’n  no. 

Oi 

^2 

^3 

e. 

1 

83.366° 

90.974° 

-8.004° 

43.134° 

3 

70.781° 

15.151° 

151.077° 

175.387° 

5 

85.417° 

16.156° 

153.212° 

-0.859° 

6 

83.447° 

87.898° 

9.268° 

-42.221° 

i 

ai  (m) 

bi  (m) 

Oli 

0,' 

1 

0.12 

0 

-5T 

9, 

2 

1.16 

0.89 

35° 

92 

3 

0.07 

0.25 

95° 

^3 

4 

0.88 

-0.43 

0 

o^ 

9a 

5 

0.39 

0.50 

0 

<40 

1 

95 

6 

0.93 

-1.34 

1 

0 

0 

96 

Fig.  9.4  Contours  Ci,  C2,  C3, 
and  C4  for  the  Li  manipulator 


64  [rad] 


"-0.357279  -0.850000  0.387106' 

'  0.798811' 

Q  = 

0.915644  -0.237000  0.324694 

,  P  = 

-0.000331 

_-0. 184246  0.470458  0.862973_ 

1.200658  _ 

where  p  is  given  in  meters.  Again,  we  start  by  dividing  {at ,  bf  and  vector  p  by  the 
characteristic  length  L,  that  was  found  to  be  L  =  890.1  mm. 

The  four  contours  obtained  with  the  bivariate-equation  approach  are  superim¬ 
posed  in  Fig.  9.4,  where,  apparently,  we  can  detect  18  intersections.  This  means  that 
at  least  two  are  spurious,  for  the  number  of  inverse-displacement  solutions  can  be,  at 
most,  16.  In  this  figure,  intersections  12  and  13  appear  as  one  single  point.  A  zoom- 
in  revealed  two  neighboring  solutions  in  a  region  around  the  said  single  point. 
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Table  9.7  Rough  estimates  of  the  coordinates  of  the  intersection  points  of  Li’s  manipulator 


Sol’n  no. 

1 

2 

3 

4 

5 

6 

7 

8 

9 

64  (rad) 

-0.56 

-1.88 

-2.09 

-2.59 

-3.07 

-1.62 

-0.72 

2.09 

2.75 

O5  (rad) 

-3.01 

-2.71 

-2.54 

-2.25 

0.21 

1.75 

2.86 

3.08 

2.5 

Sol’n  no. 

10 

11 

12 

13 

14 

15 

16 

17 

18 

O4  (rad) 

2.54 

2.36 

0.08 

0.11 

-1.22 

-2.16 

-0.36 

1.17 

0.44 

O5  (rad) 

2.37 

-0.89 

-2.23 

-2.25 

-2.51 

-0.50 

-0.70 

-0.18 

-0.12 

Table  9.8  Refined  estimates,  to  14  digits,  of  the  coordinates  of  the  18 
intersection  points  of  Fig.  9.4 


Inters ’n  no. 

O4  (rad) 

O5  (rad) 

1 

-0.5656865073441 

-3.0127341939867 

2 

-1.8817916819320 

-2.7181441928227 

3 

-2.0982054358488 

-2.5458222487325 

4 

-2.5943879129109 

-2.2563308501840 

5 

-3.0760703821644 

0.2173802902678 

6 

-1.6227073591253 

1.7564609766664 

7 

-0.7268312801527 

2.8637219341062 

8 

2.0991093946626 

3.0822214487834 

9 

2.7591234998160 

2.5875200635823 

10 

2.5458806726888 

2.3797734576690 

11 

2.3681644908739 

-0.8961886662259 

12 

0.0834264321499 

-2.2306893314165 

13 

0.1144843294210 

-2.2536422392721 

14 

-1.2262527241259 

-2.5145351139614 

15 

-2.1620940322382 

-0.5098897084087 

16 

-0.3665297041826 

-0.7057880105554 

17 

1.1793192137176 

-0.1889758121252 

18 

0.4440232934648 

-0.1282084013846 

The  coordinates  (O4,  O5)  of  each  intersection  point  are  first  estimated  by  inspection, 
as  listed  in  Table  9.7.  Further,  we  submit  each  of  these  18  values  as  the  initial  guess 
for  the  Newton-Gauss  procedure — or  Matlab’s  function  Isqnonlin — to  find  the 
least-square  approximation  of  the  overdetermined  system  of  four  equations  in  two 
unknowns  of  Eqs.  (9.62).  We  used  ODA  to  compute  the  least-square  approximation 
sought,  and  verified  the  result  with  Isqnonlin.  For  the  record,  we  include  all 
18  solutions  found,  with  14  digits,  in  radians,  in  Table  9.8.  The  18  solutions  thus 
found  were  then  used  to  compute  X3  of  Eq.  (9.70a).  As  it  turned  out,  solutions  6 
and  14  led  to  values  of  Eucledian  norm  of  vector  X3  greater  than  unity,  and  were, 
thus,  discarded  as  spurious.  The  robot  thus  admits  16  real  inverse  displacement 
solutions  at  the  given  pose.  The  legitimate  solutions  are  displayed  in  Table  9.9,  the 
values  of  the  remaining  angles  being  displayed  in  Table  9.10,  in  degrees  for  easier 
visualization. 
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Table  9.9  Legitimate  solutions  of  the  inverse  displacement  of  Li’s  manipulator  at  the  given  pose 


Sol’n  no. 

e, 

Os 

Sol’n  no.  O4 

Os 

1 

1 

o 

bo 

o 

-155.73° 

9 

0 

0 

0 

(N 

1 

-40.43° 

2 

120.25° 

176.59° 

10 

6.55° 

-129.12° 

3 

4.77° 

-127.80° 

11 

135.68° 

135.68° 

4 

158.09° 

148.26° 

12 

25.44° 

-7.34° 

5 

1 

o^ 

o 

164.07° 

13 

— 

176.07° 

11.57° 

6 

-120.21° 

-145.86° 

14 

67.57° 

0 

(N 

00 

d 

1 

7 

-32.41° 

-172.61° 

15 

— 

123.87° 

-29.21° 

8 

145.86° 

136.35° 

16 

— 

148.64° 

-148.64° 

Table  9.10  Remaining  angles  corresponding 

to  the  solutions  of  Table  9.9 

Sof 

’n  no. 

Ox  6 

'2 

03 

06 

1 

174.083°  - 

163.302° 

-164.791° 

141.281° 

2 

-159.859° 

159.324° 

-111.347° 

21.654° 

3 

164.800° 

154.290° 

-85.341° 

-101.359° 

4 

-148.749°  - 

179.740° 

-78.505° 

55.719° 

5 

-16.480° 

-10.747° 

-58.894° 

5.61T 

6 

-46.014° 

-19.256° 

-46.988° 

-114.768° 

7 

-22.260° 

-22.431° 

-32.024° 

-17.155° 

8 

-53.176° 

26.165° 

0 

OJ 

0 

127.978° 

9 

-173.928° 

150.697° 

bo 

0 

-92.284° 

10 

-41.684° 

-29.130° 

52.360° 

25.091° 

11 

-137.195° 

156.920° 

68.306° 

147.446° 

12 

-139.059° 

128.112° 

96.052° 

-119.837° 

13 

-22.696° 

29.214° 

98.631° 

170.303° 

14 

-83.094° 

57.022° 

130.976° 

-110.981° 

15 

1.227° 

-7.353° 

142.697° 

149.208° 

16 

177.538° 

148.178° 

159.429° 

110.984° 

Example  9.7.3.  In  this  example,  we  discuss  the  IDP  of  DIESTRO,  the  isotropic 
six-axis  orthogonal  manipulator  shown  in  Fig.  5.15  (Williams  et  al.  1993).  For  a 
meaning  of  kinematic  isotropy,  we  refer  the  reader  to  Sect.  5.8.  This  manipulator 
has  the  DH  parameters  given  in  Table  5.1.  The  pose  of  the  end-effector  leading  to 
an  isotropic  posture,  i.e.,  one  whose  Jacobian  matrix  is  isotropic,  is  defined  by  the 
orthogonal  matrix  Q  and  the  position  vector  p  displayed  below: 


"0 

-1  0 

o’ 

Q  = 

0 

0  -1 

,  p  = 

-50 

_1 

0 

0 

1 _ 

50_ 

with  p  given  in  mm.  Compute  all  real  inverse  displacement  solutions  at  the  given 
pose. 


9.7  Examples 


415 


Solution:  The  characteristic  length  of  DIESTRO  was  found  in  Sect.  5.8  to  be  equal 
to  the  common  value  at  =  bt  =  50  mm,  for  /  =  1, . . . ,  6.  This  manipulator,  at  the 
given  pose  of  the  EE,  exhibits  a  self-motion,  proper  of  redundant  manipulators,  but 
not  expected  in  a  six-revolute  robot.  A  self-motion  occurs  when  a  manipulator  has 
the  ability  to  move  all  its  joints  while  keeping  its  EE  fixed  at  one  given  pose.  This 
feature  makes  the  procedure  of  Sect.  9.4  difficult  to  apply. We  resort,  hence,  to 
an  alternative  approach:  We  go  back  to  Eq.  (9.27)  and  partition  it  into  two  sets  of 
equations: 


P«X45  =  R^xi2  (9.74a) 

P/X45  =  R/X12  (9.74b) 

where  P^  and  P/  are  6  x  9  and  8x9  submatrices  of  P.  Likewise,  R^  and  R/  are 
the  corresponding  6x8  and  8x8  submatrices  of  R.  In  the  above  partitioning,  the 
equations  must  be  grouped  such  that  R/  be  nonsingular.  Using  Eqs.  (9.74a  and  b), 
six  scalar  equations  free  of  0i  and  O2  can  be  derived,  namely, 

rx45  =  06,  r  ^  P„  -  R„(Rr'P/)  (9.75) 

where  65  is  the  six-dimensional  zero  vector.  Since  the  entries  of  the  6x9  matrix  F 
are  all  linear  in  X3,  the  entry  in  the  ith  row  and  7  th  column  of  the  foregoing  matrix, 
Yij ,  can  be  expressed  in  the  form 

Yij  =  aijC3  +  bijS3  +  Cij  ;  i  =  I,...,  6;  j  =  I,..., 9  (9.76) 

In  the  above  expression,  coefficients  atj ,  bfj ,  and  Ctj  are  independent  of  the  joint 
variables.  Using  Eq.  (9.76),  we  can  expand  Eq.  (9.75)  and  then  rearrange  the  terms 
in  the  i  th  equation,  thus  obtaining 

AiC3  -\-  BiS3  -\-  Ci  =  0 i  =  ,6  (9.77a) 

where,  for  f  =  1 , . . . ,  6,  we  have 


Ai 

=  aiiS4S5 

+ 

(212^4^5  +  Cli3C4Ss  +  ai4C4Cs  +  ^2/5^4  +  ai^C4 

+anS5 

+ 

+  ^i9 

(9.77b) 

Bi 

=  bilS4S5 

+ 

basics  +  bi3C4S5  +  bi^c^cs  +  btss^  +  biec^ 

+bnS5 

+ 

bi^Cs  +  big 

(9.77c) 

Ci 

=  C,li4‘55 

+ 

+  ^136483  +  Q4C4C5  +  C/5^4  +  Ci^C4 

+  Ci7‘S'5 

+ 

CisCs  +  Cig 

(9.77d) 

11 


The  self-motion  is  not  readily  detected  by  contour-intersection  using  this  procedure. 
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Now  the  six  scalar  equations  (9.77a)  are  cast  in  vector  form  as 

Dy3  =  06  (9.78) 

In  the  above  equation,  D  is  a  6  x  3  matrix  whose  entries  are  bilinear  in  X4  and  X5, 
while  y3  was  defined  in  Eq.  (9.41).  Now,  to  eliminate  O3,  we  realize  that,  from  its 
definition,  y3  ^  0,  and  hence,  D  must  be  rank-deficient.  This  means  that  every  one 
of  its  20  3  X  3  determinants,  obtained  by  picking  up  three  of  its  six  rows  at  a  time, 
should  vanish — the  number  of  combinations  of  six  objects  taking  three  at  a  time  is 
20.  We  need,  in  principle,  only  two  of  these  determinants  to  obtain  two  independent 
equations  in  ^4  and  Os.  Actually,  to  be  on  the  safe  side,  we  should  impose  the 
vanishing  of  all  20  possible  determinants,  which  would  yield,  correspondingly, 
20  contours  in  the  ^4-^5  plane;  the  intersections  of  all  contours  would  then  yield 
the  real  (^4,^5)  pairs  of  values  which  render  D  rank-deficient.  Nevertheless,  the 
visualization  of  the  intersections  of  all  20  contours  would  be  physically  impossible, 
and  so,  we  have  to  compromise  with  a  smaller  number.  As  we  have  experienced, 
only  two  of  the  above  determinants  are  prone  to  yield  spurious  solutions,  for  which 
reason  we  pick  up  a  reduced  number  of  determinants  and  derive  three  equations  in 
04  and  05. 

We  produce  the  three  desired  equations  by  first  partitioning  the  6x3  matrix  D  of 
Eq.  (9.78)  into  two  3x3  blocks,  being  the  upper,  D/  the  lower  block,  which  thus 
yields 


Ai  =  det(D^),  A2  =  det(D/) 

Now,  since  the  determinant  is  not  additive,  i.e.,  det(Df^  +  D/)  ^  det(Dj^)  +  det(D/), 
we  choose  A3  as 


A3  =  det(D«  +  D/) 


which  is  apparently  independent  of  Ai  and  A2,  thereby  obtaining  three  determi¬ 
nants,^^  which,  when  equated  to  zero,  yield  three  independent  equations  in  04  and 
05.  Each  of  these  equations  describes  a  contour  C/,  for  /  =  1,2,3,  in  the  04-05 
plane,  i.e.. 


C,-  :  F,(04,05)  =  O,  /  =  1,2,3  (9.79) 

Note  that,  by  plotting  the  three  contours  in  the  — tt  <  0/  <  tt  region,  for  i  =4,5, 
we  ensure  that  no  real  solutions  will  be  missed. 


^^This  idea  was  proposed  by  Dr.  Kourosh  Etemadi  Zanganeh,  CANMET  (Nepean,  Ontario, 
Canada). 
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Fig.  9.5  Contours  Ci,  C2,  and 

C3  for  the  DIESTRO  ^ 

manipulator  at  given  pose 

2 


1 

05  (rad)  0 

-1 

-2 

-3 


Table  9.11  Inverse  displacement  solutions  of  the  DIESTRO  manipulator 


Solution  no.  0i  62  O2,  O4.  9s  Oe 

1  0°  90°  -90°  90°  -90°  180° 

2  180°  -90°  90°  -90°  90°  0° 


04  (rad) 


The  three  contours  thus  obtained  are  plotted  in  Fig.  9.5a.  As  this  figure  shows, 
the  three  contours  intersect  at  two  isolated  points,  those  labeled  1  and  2.  The 
contours  also  intersect  along  a  curve  labeled  SS  in  the  same  figure,  which  thus 
represents  a  manifold  of  singular  solutions;  this  means  that  DIESTRO  admits  a 
set  of  self-motions.  These  motions  can  be  explained  by  noticing  that  when  the 
end-effector  is  located  at  the  given  pose  and  the  manipulator  is  postured  at  joint- 
variable  values  determined  by  any  point  on  the  SS  curve,  the  six  links  form  a 
Bricard  mechanism  (Bricard  1927).  The  degree  of  freedom  of  a  Bricard  mechanism 
cannot  be  determined  from  the  well-known  Chebyshev-Griibler-Kutzbach  formula 
(Angeles  2005),  which  yields  a  dof  =  0.  Here,  the  single-dof  motion  of  the 
mechanism  occurs  because  the  six  revolute  axes  are  laid  out  in  such  a  way  that 
if  they  are  grouped  in  two  alternating  triads,  then  these  triads  intersect. 

Furthermore,  contours  Ci  and  C2  intersect  at  right  angles  at  solution  1,  which 
corresponds  to  the  isotropic  posture  of  the  robot.  The  numerical  values  of  the  joint 
variables  for  the  isolated  solutions  are  given  in  Table  9.1 1. 

This  example  shows  interesting  features  of  the  manipulator  IDP  which  are  not 
present  in  manipulators  with  simpler  architectures,  such  as  those  with  intersecting 
or  parallel  consecutive  axes. 

Moreover,  the  point  of  coordinates  ^4  =  Os  =  7r/2  of  Fig.  9.5  appears  to  be 
an  intersection  of  the  three  contours,  and  hence,  a  solution  of  the  IDP  at  hand. 
A  close-up  of  this  point,  as  displayed  in  Fig.  9.6a,  shows  that  this  point  is  indeed  an 
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9a  (deg) 


9a  (deg) 


Fig.  9.6  A  close-up  of:  (a)  the  apparent  contour  intersection  at  the  point  of  coordinates  9/^  =  9s  = 
7X  jl  (90°);  and  (b)  the  apparent  contour  intersection  southwest  of  solution  2 


intersection  of  all  three  contours,  hut  this  point  is,  in  fact,  a  double  point,  i.e.,  a  point 
at  which  each  contour  crosses  itself;  this  gives  the  point  a  special  character:  When 
verifying  whether  this  point  is  a  solution  of  the  problem  under  study,  we  tried  to 
solve  for  X3  from  eq.  (9.70a),  but  then  found  that  matrix  H  of  that  equation  vanishes, 
and  hence,  does  not  allow  for  the  calculation  of  X3 .  An  alternative  approach  to  testing 
the  foregoing  values  of  ^4  and  Os  is  described  in  Exercise  9.5.  In  following  this 
approach,  it  was  found  that  these  values  do  not  yield  a  solution,  and  hence,  the 
intersection  point  is  discarded. 

One  more  point  that  appears  as  an  intersection  of  the  three  contours  is  that 
southwest  of  solution  2.  A  close-up  of  this  point,  as  shown  in  Fig.  9.6b,  reveals  that 
the  three  contours  do  not  intersect  in  that  region.  In  summary,  then,  the  manipulator 
at  hand  admits  two  isolated  inverse-displacement  solutions  at  the  given  pose  and  an 
infinity  of  solutions  along  the  curve  SS. 


9.8  Exercises 

9. 1  Show  that  the  left-hand  side  of  Eq.  (9.23f)  represents  a  pure  reflection  of  vector 
h  about  a  plane  of  unit  normal  f/||f||,  if  multiplied  by  ||f|p.  Also  show  that  the 
right-hand  side  of  the  same  equation  represents  a  pure  reflection  of  vector  i 
about  a  plane  of  unit  normal  g/||g||,  if  multiplied  by  ||g|p. 

9.2  Show  that  \lri  and  \[r2,  as  defined  in  Eqs.  (9.43c  and  d)  both  vanish. 

9.3  In  this  exercise,  we  will  try  to  gain  insight  into  the  consequence  of  the  double 
point  at  64  =  65  =  7112  of  Fig.  9.5  of  Example  9.7.3.  To  this  end,  show 
that,  for  this  combination  of  values,  matrix  H  of  Eq.  (9.70a)  becomes  zero, 
and  hence,  X3  cannot  be  computed  from  this  equation.  As  a  result,  none  of  the 
remaining  angles  can  be  computed  recursively. 
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9.4  As  an  alternative  approach  to  the  14  fundamental  equations  derived  in 
Sect.  9.2,  we  recall  Eqs.  (9.16a  and  b),  if  written  in  a  more  convenient  form,  so 
as  to  have  a  minimum  number  of  matrix  multiplications,  namely, 

Q3Q4Q5  =  QlQ[QQ[ 

Q2  Q[  (ai  -  p)  +  Q2  32  +  33  +  Q334 

+  Q3Q4a5  +  Q3Q4Q5a6  =  0 

Now  equate  the  four  linear  invariants  of  the  two  sides  of  the  first  of  the 
two  foregoing  equations.  The  result  is  a  set  of  four  scalar  equations.  When 
the  translational  equations  are  expanded,  and  appended  to  the  first  four 
equations,  a  system  of  seven  trigonometric  equations  in  the  six  unknown 
angles  is  derived.  Obtain  that  system  of  seven  equations  and  comment  on  their 
suitability  to  solve  the  IDP. 

9.5  In  Sect.  9.6  we  realized  that,  upon  applying  the  Raghavan-Roth  elimination 
method,  and  once  O3  is  computed,  O4  and  Os  can  be  computed  at  once  by 
finding  the  eigenvector  of  S  associated  with  its  zero  eigenvalue.  While  this 
calculation  can  be  performed  with  the  eigenvalue-computation  module  of  any 
scientific  package,  computing  the  eigenvalues  of  a  12  x  12  matrix  like  S 
requires  an  iterative  procedure,  which  can  be  time-consuming,  especially  if 
this  computation  is  only  a  part  of  a  more  complex  procedure. 

In  order  to  find  X45,  and  hence,  O4  and  Os,  from  Eq.  (9.48d),  we  need 
not  resort  to  a  full  eigenvalue  problem.  Instead,  a  vector  v  can  be  computed 
directly,  as  opposed  to  iteratively,  that  spans  the  null  space  of  S,  for  a  given 
computed  value  of  ^3,  if  a  change  of  variables  is  introduced  that  will  yield  S 
in  upper-triangular  form.  In  fact,  since  S  is  a  fortiori  singular,  its  last  row  is 
bound  to  have  zero  entries  in  that  form.  Devise  an  algorithm  that  will  render  S 
in  upper-triangular  form  and  hence,  compute  vector  X45  under  the  conditions 
that  this  vector  (a)  lie  in  the  null  space  of  S  and  (b)  its  1 2th  entry  be  unity. 
Hint:  Apply  an  orthogonalization  procedure,  as  described  in  Appendix  B. 

9.6  With  reference  to  Example  9.7.3,  keep  the  EE  of  DIESTRO  fixed  to  the 
manipulator  base  at  the  given  pose,  thereby  forming  a  6R  closed  kinematic 
chain.  Eind  the  singularity  locus  SS  of  Eig.  9.5  by  means  of  a  kinematic  input- 
output  analysis  of  the  closed  chain,  which  turns  out  to  be  a  Bricard  mechanism. 

9.7  Using  the  rough  estimates  displayed  in  Table  9.2,  Example  9.7.1,  compute 
refined  estimates  of  the  coordinates  of  intersection  point  4  upon  solving  the 
four  equations  (9.62)  pairwise  by  means  of  the  Newton-Raphson  method. 
Compute  the  condition  number  of  each  solution  based  on  the  Erobenius  norm 
of  the  2x2  Jacobian  F  of  Eq.  (9.63).  Comment  on  your  result. 

9.8  Write  a  procedure  to  compute  matrix  S  of  the  Raghavan-Roth  method.  Then, 
evaluate  this  matrix  at  solutions  7  and  8  of  Example  9.7.1. 

9.9  Derive  expressions  for  vectors  f,  g,  h  and  I  of  Eqs.  (9.33a-d). 

9.10  Derive  an  expression  for  F  4,  and  hence,  one  for  F  4X45  of  Eq.  (9.40a). 


Chapter  10 

Kinematics  of  Alternative  Robotic  Mechanical 
Systems 


10.1  Introduction 

The  study  of  robotic  mechanical  systems  has  focused,  so  far,  on  serial  manipulators. 
These  are  the  most  common  systems  of  their  kind,  but  nowadays  by  no  means 
the  majority.  In  recent  years,  other  kinds  of  robotic  mechanical  systems  have  been 
developed,  as  outlined  in  Chap.  1.  Under  alternative  robotic  mechanical  systems  we 
understand  here:  (a)  parallel  robots;  (b)  multifingered  hands;  (c)  walking  machines; 
and  (d)  rolling  robots.  A  class  that  is  increasingly  receiving  attention,  humanoids, 
portrays  an  architecture  inspired  from  the  human  musculo- skeletal  system.  This 
class  deserves  a  study  on  its  own  because  of  the  host  of  control  problems  that  it  poses 
to  the  roboticist;  its  kinematics,  however,  can  be  studied  with  the  tools  developed 
in  this  chapter  for  the  first  three  kinds  of  systems  listed  above.  For  this  reason,  a 
section  on  humanoids  is  not  included  here. 

Moreover,  under  the  general  heading  of  kinematics,  we  study  in  this  chapter  the 
geometry  and  velocity  relations  between  the  sets  of  joint  and  Cartesian  variables. 
For  the  sake  of  brevity,  however,  we  leave  aside  the  statics  of  these  systems.  The 
reader  should  be  able  to  derive  these  relations  from  the  duality  between  kinematics 
and  statics,  as  outlined  in  Chaps.  3  and  5.  Some  exercises  in  this  regard  are  included 
in  this  chapter. 


10.2  Kinematics  of  Parallel  Manipulators 

Unlike  serial  manipulators,  their  parallel  counterparts  are  composed  of  kinematic 
chains  with  closed  subchains.  A  fairly  general  parallel  manipulator  is  shown  in 
Fig.  10.1,  in  which  one  can  distinguish  two  platforms,  one  fixed  to  the  ground, 
B,  and  one  capable  of  moving  arbitrarily  within  its  workspace,  M..  The  moving 
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Fig.  10.1  A  general  six-dof  parallel  manipulator 


Fig.  10.2  A  six-dof  flight  simulator:  (a)  general  layout;  (b)  geometry  of  its  two  platforms 


platform  is  connected  to  the  fixed  platform  through  six  legs,  each  being  regarded 
as  a  six-axis  serial  manipulator  whose  base  is  B  and  whose  end-effector  is  M. .  The 
whole  leg  is  composed  of  six  links  coupled  through  six  revolutes. 

The  robotic  architecture  shown  in  Fig.  10.1  is,  in  fact,  too  general,  and  of  little 
use  as  such.  A  simpler  and  more  practical  parallel  architecture,  which  is  used  as 
a  flight  simulator,  is  sketched  in  Fig.  10.2a.  In  this  figure,  the  fixed  platform  B 
is  a  regular  hexagon,  while  the  moving  platform  M.  is  an  equilateral  triangle,  as 
depicted  in  Fig.  10.2b.  Moreover,  B  is  connected  to  M.  by  means  of  six  serial  chains. 
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Fig.  10.3  A  layout  of  a  leg  of  the  manipulator  of  Fig.  10.2 


each  comprising  five  revolutes  and  one  prismatic  pair.  Three  of  the  revolutes  bear 
concurrent  axes,  and  hence,  constitute  a  spherical  joint,  similar  to  the  wrists  studied 
in  Sect.  4.4,  while  two  more  have  axes  intersecting  at  right  angles,  thus  constituting 
a  universal  joint.  Of  the  six  foregoing  joints,  only  one,  the  prismatic  pair,  is  actuated. 

It  is  to  be  noted  that  although  each  leg  of  the  manipulator  of  Fig.  10.2a  has  a 
spherical  joint  at  only  one  end  and  a  universal  joint  at  the  other  end,  we  represent 
each  leg  in  that  figure  with  a  spherical  joint  at  each  end.  Kinematically,  the  leg 
depicted  in  Fig.  10.2a  is  equivalent  to  the  actual  one,  the  only  difference  being  that 
the  former  appears  to  have  a  redundant  joint.  We  use  the  model  of  Fig.  10.2a  only 
to  make  the  drawing  simpler.  A  more  accurate  display  of  the  leg  architecture  of  this 
manipulator  appears  in  Fig.  10.3. 

Because  the  kinematics  and  statics  of  parallel  manipulators  at  large  are  beyond 
the  scope  of  this  book,  we  will  limit  the  discussion  to  parallel  manipulators  of  the 
simplest  type. 

With  regard  to  the  manipulators  under  study,  we  can  also  distinguish  between 
the  inverse  and  the  direct  kinematics  problems  in  exactly  the  same  way  as  these 
problems  were  defined  for  serial  manipulators.  The  inverse  kinematics  of  the  general 
manipulator  of  Fig.  10.1  is  identical  to  that  of  the  general  serial  manipulator  studied 
in  Sect.  9.2.  In  fact,  each  leg  can  be  studied  separately  for  this  purpose,  the  problem 
thus  becoming  the  same  as  in  that  section.  For  the  particular  architecture  of  the 
manipulator  of  Fig.  10.2a,  in  which  the  actuated  joint  variables  are  displacements 
measured  along  the  leg  axes,  the  inverse  kinematics  simplifies  substantially  and 
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allows  for  a  simple  closed-form  solution.  However,  the  direct  kinematics  of  the 
same  manipulator  is  as  challenging  as  that  of  the  general  serial  manipulator 
of  Sect.  9.2.  With  regard  to  the  direct  kinematics  of  manipulators  of  the  type 
depicted  in  Fig.  10.2a,  Charentus  and  Renaud  (1989)  and  Nanua  et  al.  (1990) 
showed  independently  that  like  the  inverse  kinematics  of  general  six-axis  serial 
manipulators,  the  direct  kinematics  of  this  manipulator  reduces  to  a  16th-degree 
polynomial.  Note,  however,  that  the  direct  kinematics  of  a  manipulator  similar  to 
that  of  Fig.  10.2a,  but  with  arbitrary  locations  of  the  attachment  points  of  each 
leg  to  the  moving  and  fixed  platforms,  termed  the  general  platform  manipulator, 
has  been  the  subject  of  intensive  research  (Merlet  2006).  A  breakthrough  in  the 
solution  of  the  direct  kinematics  of  platform  manipulators  of  the  general  type 
was  reported  by  Raghavan  (1993),  who  resorted  to  polynomial  continuation,  a 
technique  already  mentioned  in  Sect.  9.2,  for  computing  up  to  40  poses  of  M. 
for  given  leg  lengths  of  a  parallel  manipulator  with  legs  of  the  type  depicted  in 
Fig.  10.3,  but  with  attachment  points  at  both  M  and  B  with  an  arbitrary  layout. 
What  Raghavan  did  not  derive  is  the  characteristic  40th-degree  polynomial  of  the 
general  platform  manipulator.  Independently,  Wampler  (1996)  and  Husty  (1996) 
devised  procedures  to  derive  this  polynomial,  although  Wampler  did  not  pursue 
the  univariate  polynomial  approach  and  preferred  to  cast  the  problem  in  a  form 
suitable  for  its  solution  by  means  of  polynomial  continuation.  Husty  did  derive 
the  40th-degree  polynomial  for  several  examples.  In  the  process,  he  showed  that 
this  polynomial  is  the  underlying  characteristic  polynomial  for  all  manipulators 
of  the  platform  type,  which  simplifies  to  a  lower-degree  polynomial  for  simpler 
architectures.  As  a  matter  of  fact,  Lee  and  Roth  (1993)  solved  the  direct  kinematics 
of  platform  manipulators  for  which  the  attachment  points  at  the  base  and  the  moving 
platforms  are  located  at  the  vertices  of  planar,  similar  hexagons.  These  researchers 
showed  that  the  problem  here  reduces  to  a  cascade  of  quadratic  and  linear  equations. 
In  the  particular  case  in  which  both  polygons  are  regular,  however,  the  manipulator 
degenerates  into  a  movable  structure,  upon  fixing  the  leg  lengths,  and  hence,  the 
solution  set  becomes  a  continuum.  Hazard  and  Merlet  (1994),  in  turn,  showed  that 
the  platform  manipulator  originally  proposed  by  Stewart  (1965),  and  known  as  the 
Stewart-Gough  platform,  has  a  12th-degree  characteristic  polynomial.  Interestingly, 
these  mechanical  systems  were  first  introduced  by  Gough  (1956-1957)  for  testing 
tires;  Stewart  (1965)  suggested  their  use  as  flight  simulators,  an  application  that  is 
now  well  established. 

Husty,  however,  did  not  show  that  his  40th-degree  polynomial  is  minimal  in 
that  manipulator  architectures  are  possible  that  exhibit  up  to  40  actual  solutions. 
Dietmaier  (1998)  did  this,  by  devising  an  algorithm  that  would  iteratively  increase 
the  number  of  real  solutions  of  a  given  architecture.  With  this  paper,  Dietmaier 
proved  conclusively  that  Husty ’s  40th-degree  polynomial  is,  in  fact,  minimal.  This 
was  rather  surprising,  for  virtually  everybody  working  in  the  field  expected  a 
minimal  polynomial  of  a  degree  of  the  form  2",  with  n  being  a  positive  integer. 
Notice  that,  in  the  cases  of  the  most  general  serial  six-revolute  manipulator  and  of 
the  flight  simulator,  the  minimal  polynomial  is  of  a  degree  of  this  form,  with  n  =  4. 
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Table  10.1  DH  parameters 
of  the  leg  of  Fig.  10.3 


i 

bi 

Oli 

1 

0 

0 

90° 

2 

0 

0 

90° 

3 

0 

bz 

0° 

4 

0 

bi  (const) 

90° 

5 

0 

0 

90° 

6 

0 

be  (const) 

0° 

Below  we  analyze  the  inverse  kinematics  of  one  leg  of  the  manipulator  of 
Fig.  10.2a,  as  depicted  in  Fig.  10.3.  The  Denavit-Hartenberg  parameters  of  the  leg 
shown  in  this  figure  are  given  in  Table  10. 1 .  It  is  apparent  that  the  leg  under  study  is  a 
decoupled  manipulator.  Its  inverse  kinematics  can  be  derived  by  properly  modifying 
the  scheme  introduced  in  Sect.  4.4,  for  we  now  have  a  prismatic  joint,  which  is,  in 
fact,  the  only  active  joint  of  this  manipulator.  Moreover,  by  virtue  of  the  underlying 
design,  the  active  joint  variable,  can  take  on  only  positive  values. 

In  view  of  the  DH  parameters  of  this  manipulator,  Eq.  (4.16)  reduces  to 


QiQ2(a3  +  34)  =  C  (10.1) 

where  c  denotes  the  position  vector  of  the  center  C  of  the  spherical  wrist  and,  since 
frames  2^3  and  of  the  DH  notation  are  related  by  a  pure  translation,  Q3  =  1. 
Upon  equating  the  squares  of  the  Euclidean  norms  of  both  sides  of  the  foregoing 
equation,  we  obtain 


a3  +  a4 


where,  by  virtue  of  the  DH  parameters  of  Table  10.1, 


a3  +  a4 


(10.2) 


Now,  since  both  b^,  and  b/[  are  positive  by  construction,  Eq.  (10.2)  readily  leads  to 
the  desired  inverse  kinematics  solution,  namely. 


b 


3 


—  Z74  >  0 


(10.3) 


a  result  that  could  have  been  derived  by  inspection  of  Fig.  10.3. 

Note  that  the  remaining  five  joint  variables  of  the  leg  under  study  are  not 
needed  for  purposes  of  inverse  kinematics,  and  hence,  their  calculation  could  be 
skipped.  However,  in  studying  the  differential  kinematics  of  these  manipulators, 
these  variables  will  be  needed;  it  is  thus  convenient  to  solve  for  them  now.  This 
is  straightforward,  as  shown  below:  Upon  expansion  of  Eq.  (10.1),  we  derive  three 
scalar  equations  in  two  unknowns,  0i  and  O2,  namely. 
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(bs  +  bi)s2  =  xcci  +  jc^i 

(10.4a) 

(Z)3  +  Z)4)C2  =  ZC 

(10.4b) 

0  =  xcSi  -  ycCi 

(10.4c) 

in  which  c,  and  Si  stand  for  cos  Of  and  sin  Oi ,  respectively,  while  occurring  in  the 
above  equations,  is  available  in  Eq.  (10.3).  From  Eq.  (10.4c),  0i  is  derived  as 

0i  =  tan“^  (10.5a) 


which  yields  a  unique  value  of  0i  rather  than  the  two  lying  ic  radians  apart,  for 
the  two  coordinates  xc  and  yc  determine  the  quadrant  in  which  0i  lies.  Once  0i  is 
known,  O2  is  derived  uniquely  from  the  remaining  two  equations  through  its  cosine 
and  sine  functions,  i.e.. 


^^2  = 


ZC 


+  ^4 


^2  = 


Z?3  +  Z74 


(10.5b) 


With  the  first  three  joint  variables  of  this  leg  known,  the  remaining  ones,  i.e.,  those 
of  the  “wrist,”  are  calculated  as  described  in  Sect.  4.4.2.  Therefore,  the  inverse 
kinematics  of  each  leg  admits  two  solutions,  one  for  the  first  three  variables  and 
two  for  the  last  three.  Moreover,  since  the  only  actuated  joint  is  one  of  the  first 
three,  which  of  the  two  wrist  solutions  is  chosen  does  not  affect  the  value  of  b^ ,  and 
hence,  each  manipulator  leg  admits  only  one  inverse  kinematics  solution. 

While  the  inverse  kinematics  of  this  leg  is  quite  straightforward,  its  direct 
kinematics  is  not.  Below  we  give  an  outline  of  the  solution  procedure  for  the 
manipulator  under  study  that  follows  the  procedure  proposed  by  Nanua  et  al.  (1990). 

In  Fig.  10.2a,  consider  the  triangles  A/  Si  Bf ,  for  /  =  1,2,3,  where  the  subscript  i 
stands  for  the  i  th  pair  of  legs.  When  the  lengths  of  the  six  legs  are  fixed  and  plate  M. 
is  removed,  triangle  Af  Si  Bi  can  only  rotate  about  the  axis  Ai  Bi .  Therefore,  we  can 
replace  the  pair  of  legs  of  lengths  qia  and  qn,  by  a  single  leg  of  length  // ,  connected  to 
the  base  plate  S  by  a  revolute  joint  with  its  axis  along  Ai  Bi .  The  resulting  simplified 
structure,  as  shown  in  Fig.  10.4,  is  kinematically  equivalent  to  the  original  structure 
in  Fig.  10.2a. 

Now  we  introduce  the  coordinate  frame  Ti ,  with  origin  at  the  attachment  point 
Oi  of  the  i  th  leg  with  the  base  plate  B,  according  with  the  geometry  of  Fig.  10.4  and 
the  notation  below: 

For  i  =  1,2,3, 

Xi  is  directed  from  Ai  to  Bi ; 

Yi  is  chosen  such  that  is  perpendicular  to  the  plane  of  the  hexagonal  base  and 
points  upwards. 

Oi  is  set  at  the  intersection  of  Xi  and  Yi ,  and  is  designated  the  center  of  the  revolute 
joint; 


10.2  Kinematics  of  Parallel  Manipulators 


427 


Fig.  10.4  Equivalent  simplified  mechanism 


Fig.  10.5  Replacing  each 
pair  of  legs  with  a  single  leg 


Next,  we  locate  the  three  vertices  S2,  and  S3  of  the  triangular  plate  with 
position  vectors  stemming  from  the  center  O  of  the  hexagon.  Furthermore,  we  need 
to  determine  It  and  Of .  Referring  to  Figs.  10.4  and  10.5,  and  letting  a/  and  b/  denote 
the  position  vectors  of  points  Ai  and  Bf ,  respectively,  we  have 

II b/  -  a,- 1 

df  +  qfa  -  qfb 

2di 

‘ifa  - 

b,  -  a, 

df 
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Fig.  10.6  Relation  between 
frames  24)  and  24 


for  /  =  1, 2,  3,  and  hence,  u/  is  the  unit  vector  directed  from  Ai  to  Bf.  Moreover, 
the  position  of  the  origin  Of  is  given  by  vector  O/ ,  as  indicated  below: 

o,  =  2ii  +  r/U/ ,  for  i  =  1,2,3.  (10.6) 


Furthermore,  let  be  the  position  vector  of  St  in  frame  Ti  {Oi ,  Xi ,  Yi ,  Z,  ).  Then 


s/ 


0 

//  cos  (pi  , 
U  sin  (pi  _ 


for/  =  1,2,3  (10.7) 


Now  a  frame  To  (O,  X,  Y,  Z)  is  defined  with  origin  at  O  and  axes  X  and  Y 
in  the  plane  of  the  base  hexagon,  and  related  to  Xi  and  Yi  as  depicted  in  Fig.  10.6. 
When  expressed  in  frame  To,  Si  takes  on  the  form 


[s,-  ]o  =  [Oi  ]o  +  [Rz  ]oSz,  for  /  =  1,2,3  (10.8) 

where  [  R;  ]o  is  the  matrix  that  rotates  frame  To  to  frame  Ti ,  expressed  in  To,  and  is 
given  as 


cos  ai  —  sin  ai  0 
sin  ai  cos  ai  0 
0  0  1 


for/  =  1,2,3  (10.9) 


cos  ai  =  u/  •  i  =  Uix 


Referring  to  Fig.  10.6, 


sm  ai  =  U;  •  j  =  Uiy 


(10.10) 

(10.11) 
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After  substitution  of  Eqs.  (10.9)-(10.1 1)  into  Eq.  (10.8),  we  obtain 


[Si  ]o  —  [t)i  ]o  ”t“  li 


Uiy  COS  (pi 
-Uix  COS  (pi  , 
sin  (pi 


fori  =  1,2,3  (10.12) 


where  o,  is  given  by  Eq.  (10.6). 

Since  the  distances  between  the  three  vertices  of  the  triangular  plate  are  fixed, 
the  position  vectors  Si ,  S2,  and  S3  must  satisfy  the  constraints  below: 


S2  -  Si 
S3-S2 
Si  -  S3 


(10.13a) 

(10.13b) 

(10.13c) 


After  expansion,  Eqs.  (10.13a-c)  take  the  forms: 


D\C(p\  +  D2C(p2  +  D2C(piC(p2  +  D4S(piS(p2  +  —  0  (10.14a) 

EiC(p2  +  E2C(p2,  +  E2,C(p2C(p2  +  E4S(p2S(p3  +  £’5  =  0  (10.14b) 

EiCipi  +  E2C(p3  +  E2C(piC<p3  +  E4S(p\S(p3  +  £5  =  0  (10.14c) 

where  c(-)  and  s{-)  stand  for  cos(-)  and  sin(-),  respectively,  while  coefficients 

{Di ,  Ei ,  £,  }j  are  functions  of  the  data  only  and  bear  the  forms  shown  below' : 


£1 

£2 

£3 

£4 

£5 


2/1(02  -  Oi)^Eui 
-2/2(02  -Oi)^EU2 

— 2/1/2U2  Ui 


— 2/1/; 


02  I  +  I  01 


2o[o2  +  /f  +  ll-a\ 


Ei 

£2 

£3 

£4 

£5 


2/2(03  -  02)^EU2 
-2/3(03  -02)^EU3 
-2/2/30^02 


- 2/9/ 


2^3 


O3  I  +  I  O2 


2o^ O2  “1“  ^2  ^3  —  ^2 


^  Since  all  vectors  in  the  15  coefficients  of  interest  are  coplanar,  they  are  regarded  as  two- 
dimensional  vectors  in  the  display  of  the  15  coefficients. 
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Fi  =  2/i(oi  -03)^Eui 
F2  =  -2/3(01  -03)^EU3 

F3  =  -2/i/3U^Ui 


2o^oi  +  if  +  if  —af 


In  the  above  relations  the  2x2  matrix  E  is  defined  as  in  Eq.  (5.55),  and  the  frame  in 
which  the  vectors  are  expressed  is  immaterial,  as  long  as  all  vectors  appearing  in 
the  same  scalar  product  are  expressed  in  the  same  frame.  Since  expressions  for 
these  vectors  in  Tq  have  already  been  derived,  it  is  just  simpler  to  perform  those 
computations  in  this  frame. 

Our  next  step  is  to  reduce  the  foregoing  system  of  three  equations  in  three 
unknowns  to  two  equations  in  two  unknowns,  and  hence,  obtain  two  contours  in 
the  plane  of  two  of  the  three  unknowns,  the  desired  solutions  being  determined  as 
the  intersections  of  the  two  contours.  Since  Eq.  (10.14a)  is  already  free  of  03,  all 
we  have  to  do  is  eliminate  03  from  equations  (10.14b)  and  (10.14c).  To  do  this,  we 
resort  to  the  usual  trigonometric  identities  relating  c03  and  S(p3  with  tan (03/2),  in 
Eqs.  (10.14b)  and  (10.14c).  After  we  have  cleared  the  denominators  by  multiplying 
the  two  foregoing  equations  by  (1  +  r|),  the  equations  thus  resulting  take  on 
the  forms 


-\-  /^2^3  H”  ^3  —  0  (10.15a) 

r\ 

miT^  +  1712X3  +  m3  =  0  (10.15b) 

where  k\,  k2,  and  k^  are  linear  combinations  of  ^02,  c02,  and  1.  Likewise,  mi,  m2, 
and  m3  are  linear  combinations  of  ^0i,  c0i,  and  1,  namely, 

ki  =  EiC^2  -  E2-  E3C(p2  +  E5 
k2  =  2E4S(p2 

k3  =  E\C(f)2  ~\~  E2  £”3(302  £5 

mi  =  £ic0i  -  £2  -  £3(301  +  £5 
m2  =  2£4^0i 

m3  =  £iC0i  +  £2  +  £3(301  +  £5 

Next,  we  eliminate  X3  from  the  above  equations  dialytically,  as  we  did  in 
Sect.  5.4.1  to  find  the  workspace  of  a  three-axis  serial  manipulator.  We  proceed  now 
by  multiplying  each  of  the  above  equations  by  X3  to  obtain  two  more  equations, 
namely, 

k\xl  +  k2xl  +  k3X3  =  0  (10.15c) 

mir|  +  1712X2  +  1713X3  =  0  (10.15d) 
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Further,  we  write  Eqs.  (10.15a)-(10.15d)  in  homogeneous  form: 

Ot3  =  0  (10.16a) 


with  the  4x4  matrix  O  and  the  four-dimensional  vector  T3  defined  as 


1 

0 

m 

CA 

1 _ 

0  = 

mi  m2  m2  0 

0  ki  k2  k^ 

,  ^3  = 

r2 

^3 

(10.16b) 

_  0  mi  m2  m2  _ 

_  1  _ 

Equation  (10.16a)  constitutes  a  linear  homogeneous  system.  Moreover,  in  view 
of  the  form  of  vector  T3,  we  are  interested  only  in  nontrivial  solutions,  which  exist 
only  if  det(O)  vanishes.  We  thus  have  the  condition 


det(O)  =  0  (10.16c) 

Equations  (10.14a)  and  (10.16c)  form  a  system  of  two  equations  in  two 
unknowns,  (pi  and  (p2.  These  two  equations  can  be  further  reduced  to  a  single 
16th-degree  polynomial  equation  (Nanua  et  al.  1990),  as  discussed  later  on. 

In  the  spirit  of  the  contour  method  introduced  earlier,  we  plot  these  two  equations 
as  two  contours  in  the  (p\-(p2  plane  and  determine  the  desired  solutions  at  points 
where  the  two  contours  intersect.  Once  a  pair  of  (jpi ,  <p2)  values  is  found,  <p2  can 
be  uniquely  determined  from  Eqs.  (10.14b  and  c).  Indeed,  these  equations  can  be 
arranged  in  the  form: 


E^S<p2  E2  +  E3C(p2 

'  S<p3  ■ 

' -EiC(p2  -  Es' 

_  FiS^i  F2  +  F3C(j)i  _ 

_C(t>3_ 

_  -Ficcpx  -  Fs  _ 

From  the  above  equation,  both  c03  and  ^03  can  be  found  uniquely',  with  the 
foregoing  unique  values,  03  is  determined  uniquely  as  well. 

Knowing  the  angles  0i,  02,  and  03  allows  us  to  determine  the  position  vectors 
of  the  three  vertices  of  the  mobile  plate,  Si,  S2,  and  S3,  whose  expressions  are  given 
by  Eq.  (10.12).  Since  three  points  define  a  plane,  the  pose  of  the  end-effector  is 
uniquely  determined  by  the  positions  of  its  three  vertices.  We  illustrate  the  foregoing 
procedure  with  a  numerical  example  below: 

Example  10.2.1  (A  Contour-Intersection  Approach).  We  derive  the  direct  kinemat¬ 
ics  of  a  manipulator  analyzed  by  Nanua  et  al.  (1990).  This  is  a  platform  manipulator 
whose  base  plate  has  six  vertices  with  coordinates  expressed  with  respect  to  the 
fixed  reference  frame  T'o  as  given  below,  with  all  data  given  in  meters: 


Al  =  (-2.9, -0.9)  , 
A2  =  (  2.5,  4.1), 
A3  =  (  1.3. -2.3), 


Bi  =  (-1.2,  3.0) 
B2  =  (  3.2,  1.0) 
B3  =  (-1.2, -3.7) 
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Fig.  10.7  Contours  C\  and  C2 
for  Nanua  et  al.’s  example 


01  (rad) 


The  dimensions  of  the  movable  triangular  plate  are,  in  turn, 

ai  =  2.0,  a2  =  2.0,  =  3.0 

Determine  all  possible  poses  of  the  moving  plate  for  the  six  leg-lengths  given  as 

Q_\a  —  5.0,  —  5.5,  —  5.7, 

qib  =  4.5,  q2b  =  5.0,  q^b  =  5.5 

Solution:  After  substitution  of  the  given  numerical  values,  Eqs.  (10.14a) 
and  (10.16c)  become,  with  c/  and  Si  standing  for  cos0/  and  sin0;  ,  respectively, 

Ci\  61.848  -  36.9561C1  -  47.2376^2  +  33.603ciC2  -  41.6822^1^2  =  0 
C2.  -28.5721  +  48.6506ci  -  20.7097cf  +  68.7942c2  -  100.81  IciCz 

+35.9634cfc2  -  41.4096cf  +  50.8539cicf  -  15.613cfcf  -  52.9789if 
+67.6522c2if  —  \2i.2165c\sl  +  74.1623j'ii2  —  25.6617ciJ'ii2 

— 67.953c2i'ii'2  +  33.9241ciC2iii'2  —  13.202i| 

-3.75189ci5^  +  6.13542cf4  =  0 

The  foregoing  equations  determine  contours  Ci  and  C2  in  the  0i-02  plane,  which 
are  plotted  in  Fig.  10.7.  Four  real  solutions  are  found  by  superimposing  Ci  and  C2, 
as  shown  in  this  figure.  The  numerical  values  of  the  solutions,  listed  in  Table  10.2, 
agree  with  the  published  results.  Solutions  1  and  2  represent  two  poses  of  the 
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Table  10.2  Solutions  for 
Nanua  et  al.’s  example 


No. 

01  (rad) 

02  (rad) 

03  (rad) 

1 

0.8335 

0.5399 

0.8528 

2 

1.5344 

0.5107 

0.2712 

3 

-0.8335 

-0.5399 

-0.8528 

4 

-1.5344 

-0.5107 

-0.2712 

triangular  plate  over  the  base,  while  solutions  3  and  4  are  just  the  reflections  of 

solutions  1  and  2  with  respect  to  the  plane  of  the  base  plate.  Hence,  the  geometric 

symmetry  gives  rise  to  an  algebraic  symmetry  of  the  solutions. 

Example  10.2.2  (The  Univariate  Polynomial  Approach).  Reduce  the  two  equations 
found  in  Example  10.2.1,  Eqs.  (10.14a)  and  (10.16c),  to  a  single  monovariate 
polynomial  equation. 

Solution:  We  first  substitute  the  trigonometric  identities  relating  c(pi  and  s(pi  with 
Ti  =  tan(0//2),  for  /  =  1,  2,  into  Eqs.  (10.14a)  and  (10.16c).  Upon  clearing  the 
denominators  by  multiplying  those  equations  by  (1  +  r^^)(l  +  r|),  we  obtain  two 
polynomial  equations  in  ri ,  namely, 

Girf  +  02^1  +  +  04X1  G5  =  0  (10.17) 

7/1  rf  +  H2X1  +  7/3=0  (10.18) 


where 


and 


Gi  =  Kix^  +  K2rl  +  K2 
G2  =  K4X2  +  KsX2 
G3  =  Kex^  + 

G4  =  K^X2  +  TfioU 

G5  =  K11X2  +  K12X2  +  7^13 


Hi  =  L1X2  +  L2 
H2  =  L2X2 
7/3  =  L4X2  +  7.5 

In  the  above  relations,  {7f/}P  and  {Li}\  are  all  functions  of  the  data.  We  now 
eliminate  Xi  from  Eqs.  (10.17)  and  (10.18),  following  Bezout’s  method,  as  given  in 
(Salmon  1964).  To  do  this,  we  multiply  Eq.  (10.17)  by  Hi  and  Eq.  (10.18)  by  Girp 
and  subtract  the  two  equations  thus  resulting,  which  leads  to  a  cubic  equation  in  ri, 
namely. 


{G2H1  -  G,H2)xI  +  (G3H1  -  Gi7f3)4  +  G4G1T1  +  GsH,  =  0  (10.19a) 
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Likewise,  if  Eq.  (10.17)  is  multiplied  by  HiXi  +  H2  and  Eq.  (10.18)  by  Gixl  +  G2XI 
and  the  equations  thus  resulting  are  subtracted  from  each  other,  one  more  cubic 
equation  in  xi  is  obtained,  namely, 

(Gii/3  -  +  {G^H,  +  G3//2  -  G2i/3)4 

HGsHi  +  G^H2)xi  +  G5//2  =  0  (10.19b) 

Moreover,  if  we  multiply  Eq.  (10.18)  by  ri,  a  third  cubic  equation  in  Xi  can  be 
derived,  i.e.. 


H^xl  +  H2xf  +  H3X1  =  0  (10.19c) 

Now,  Eqs.  (10.18)  and  (10.19a-c)  constitute  a  homogeneous  linear  system  of  four 
equations  in  the  first  four  powers  of  Xi ,  which  can  be  cast  in  the  form 


Hti  =  0 


(10.20) 


where  r  1 


G2H1  -  G1H2  G3H1  -  G1H3  G4H1 

G3H1  —  G1H3  G3H2  —  G2H3  G/^Hi  G2XH2  ~\~  G^Hi 


G5H1- 

G5H2 


Hi  H2  H3  0 

0  Hi  H2  H3 


In  order  for  Eq.  (10.20)  to  admit  a  nontrivial  solution,  the  determinant  of  its 
coefficient  matrix  must  vanish,  i.e.. 


det(H)  =  0  (10.21) 

Given  the  definitions  of  {Gk}\  and  {Hk}\,  it  is  apparent  that  Gi,  G3,  and  G5  are 
quartic,  while  G2  and  G4  are  cubic  polynomials  in  X2.  Likewise,  Hi  and  H3  are 
quadratic,  while  H2  is  linear  in  X2  as  well.  As  a  result,  the  highest-degree  entries  of 
the  first  and  second  rows  of  H  are  sextic,  while  those  of  its  third  and  fourth  rows  are 
quadratic.  The  outcome  is  that  det(H)  is  of  degree  6  +  6  +  2  +  2=  16,  i.e.,  det(H)  is 
a  16th-degree  polynomial  in  X2.  This  equation,  in  general,  admits  up  to  16  different 
solutions.  Eurthermore,  the  roots  of  the  polynomial  appear  in  the  form  of  either 
complex  conjugate  pairs  or  real  pairs.  In  the  latter  case,  each  pair  represents  two 
symmetric  positions  of  the  mobile  platform  with  respect  to  the  base,  i.e.,  for  each 
solution  found  above  the  base,  another,  mirror-imaged,  solution  exists  below  it.  This 
symmetry  exists,  in  general,  as  long  as  the  six  base  attachment  points  are  coplanar. 

Other  parallel  manipulators  are  the  planar  and  spherical  counterparts  of  that  stud¬ 
ied  above,  and  sketched  in  Eigs.  10.8  and  10.9.  The  direct  kinematics  of  the  manip¬ 
ulator  of  Eig.  10.8  was  found  to  admit  up  to  six  real  solutions  (Gosselin  et  al.  1992), 
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Fig.  10.8  A  planar  parallel  manipulator 


Fig.  10.9  A  spherical  parallel  manipulator 


while  the  spherical  manipulator  of  Fig.  10.9  has  been  found  to  admit  up  to  eight 
direct  kinematic  solutions  (Gosselin  et  al.  1994a, b).  A  comprehensive  account  of  the 
simulation  and  design  of  three-dof  spherical  parallel  manipulators,  which  includes 
workspace  analysis  as  well,  is  included  in  (Gosselin  et  al.  1995). 
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1 0.2. 1  Velocity  and  Acceleration  Analyses 
of  Parallel  Manipulators 

Now  we  proceed  to  the  velocity  analysis  of  the  manipulator  of  Fig.  10.2a.  The 
inverse  velocity  analysis  of  this  manipulator  consists  in  determining  the  six  rates 
of  the  active  joints,  {bk}i,  given  the  twist  t  of  the  moving  platform.  The  velocity 
analysis  of  a  typical  leg  leads  to  a  relation  of  the  form  of  Eq.  (5.9),  namely, 

Jj0j=tj^  J  =  I,  II,  (10.22a) 

where  jj  is  the  Jacobian  of  the  Jth  leg,  ^  /  is  the  six-dimensional  joint-rate  vector 
of  the  same  leg,  and  tj  is  the  twist  of  the  moving  platform  AJ,  with  its  operation 
point  defined  as  the  point  Cj  of  concurrency  of  the  three  revolutes  composing  the 
spherical  joint  of  attachment  of  the  leg  to  the  moving  platform  AJ,  and  shown  in 
Fig.  10.3  as  C,  subscript  J  indicating  that  point  C  of  that  figure  is  different  for 
different  legs.  We  thus  have 


01  02  0  04  05  06 

/?3401  X  03  Z73402  X  03  03  0  0  0 


(O 

C/ 


Z734  =  Z73  +  Z74 


where  the  leg  geometry  has  been  taken  into  account. 
Furthermore,  from  Fig.  10.3,  it  is  apparent  that 


0/  =  p  -  0)  X  r/ 


(10.22b) 

(10.22c) 


(10.23) 


with  Yj  defined  as  the  vector  directed  from  C/  to  the  operation  point  P  of  the 
moving  platform. 

Now,  we  regard  the  axes  of  the  five  revolute  joints  of  the  six-joint  leg  of  Fig.  10.3 
as  a  set  of  five  zero-pitch  screws  S  =  {si ,  S2,  S4,  S5, 85}.  The  line  passing  through  Oi 
and  C  is  a  zero-pitch  screw  S3  reciprocal  to  <S,  as  shown  below: 

Since  S3  passes  through  Oi,  its  moment  with  respect  to  this  point  vanishes,  and 
hence. 


S3  = 


e3 

0 


Now  it  is  simple  matter  to  show  that 

S3  Tsfc  =  (rssl^Sfc  =  0  for  ^  =  1,2, 4, 5, 6 


(10.24) 


and  r  given  in  Eq.  (3.112),  q.e.d. 
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Notice  that,  for  the  /th  leg, 


j 


and  hence,  on  the  one  hand. 


j  =  (P^)j 


where  the  subscript  J  reminds  us  that  is  different  for  each  leg.  In  order  to  ease 
the  notation,  and  since  we  have  a  single  variable  per  leg,  we  define  henceforth 

bj  ^  {b^)j  (10.25a) 

and  hence,  the  above  relation  between  t j  and  the  actuated  joint  rate  of  the  /th  leg 
takes  the  form 


\^jijh=bj  (10.25b) 

On  the  other  hand, 

fjij  =  (el)jcj 

Likewise,  we  define 

(e3)/  =  e/  (10.26a) 

the  foregoing  relation  thus  yielding 

l^jtj  =  e^jCj  (10.26b) 

Note  that  vectors  e  j  and  r  j  define  uniquely  the  line  along  the  two  attachment  points 
of  the  /th  leg.  Henceforth,  this  line  will  be  termed  the  axis  of  the  /th  leg. 

Upon  equating  the  right-hand  sides  of  Eqs.  (10.25b)  and  (10.26b),  the  desired 
expression  for  the  actuated  joint  rate  is  derived,  namely, 

bj=e^jCj,  J  =  (10.27a) 

That  is,  the  /th  joint  rate  is  nothing  but  the  projection  onto  the  /th  leg  axis  of  the 
velocity  of  point  Cj.  Furthermore,  upon  substituting  Eq.  (10.23)  into  Eq.  (10.27a) 
above,  we  obtain  the  relations  between  the  actuated  joint  rates  and  the  twist  of  the 
moving  platform,  namely. 


[(e/  X  rj) 


e}] 


T 


(O 

.PJ 


(10.27b) 
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for  J  =  I,  II, . . .  ,VI .  Upon  assembling  all  six  leg-equations  of  Eq.  (10.27b),  we 
obtain  the  desired  relation  between  the  vector  of  actuated  joint  rates  and  the  twist  of 
the  moving  platform,  namely, 

b  =  Kt  (10.28a) 

with  the  six-dimensional  vectors  b  and  t  defined  as  the  vector  of  joint  variables  and 
the  twist  of  the  platform  at  the  operation  point,  respectively.  Moreover,  the  6x6 
matrix  K  is  the  Jacobian  of  the  manipulator  at  hand.  These  quantities  are  displayed 
below: 


1 

_ 1 

(e/  X  nY  e[ 

bn 

(O 

,  K  = 

(e//  X  r//)^  Vjj 

,  i  = 

• 

LpJ 

• 

Jbvi. 

_{gvi  X  ^VlY 

(10.28b) 


From  the  above  display,  it  is  apparent  that  each  row  of  K  is  the  transpose  of  the 
Pliicker  array  of  the  corresponding  leg  axis,  although  in  axis  coordinates,  as  opposed 
to  the  Jacobian  matrix  J  of  serial  manipulators,  whose  columns  are  the  Pliicker 
coordinates  of  the  corresponding  joint  axis  in  ray  coordinates.  Moreover,  in  these 
coordinates,  the  moment  of  the  leg-axis  is  taken  with  respect  to  the  operation  point 
P  of  M..  One  more  difference  between  the  velocity  analysis  of  serial  and  parallel 
manipulators  is  the  role  played  by  the  actuator  joint  rates  in  the  underlying  forward 
and  direct  kinematics.  In  the  case  of  parallel  manipulators,  this  role  is  changed,  for 
now  we  have  that  the  actuator  joint  rates  are  given  by  explicit  formulas  in  terms 
of  the  twist  of  the  moving  platform,  along  with  the  manipulator  architecture  and 
configuration.  Finding  the  platform  twist  requires  inverting  matrix  K.  Moreover, 
the  significance  of  singularities  also  changes:  When  K  becomes  singular,  some 
instantaneous  motions  of  the  platform  are  possible  even  if  all  actuated  joints  are 
kept  locked.  That  is,  a  singularity  of  K  is  to  be  interpreted  now  as  the  inability  of 
the  manipulator  to  withstand  a  certain  static  wrench.  An  extensive  analysis  of  the 
singularities  of  parallel  manipulators  using  line  geometry  in  a  form  that  is  known  as 
Grassmann  geometry  was  reported  by  Merlet  (1989). 

Now,  the  acceleration  analysis  of  the  same  leg  is  straightforward.  Indeed,  upon 
differentiation  of  both  sides  of  Eq.  (10.28a)  with  respect  to  time,  one  obtains 

b  =  Kt  +  Kt  (10.29a) 


where  K  takes  the  form 


r  •  r  •  r  n 

.  T  •  T 


u 


T 

VI 


e 


T 

K/-I 


K  = 


(10.29b) 
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and  uj  is  defined  as 


U/  =  0/  X  Vj 


(10.29c) 


Therefore, 


u/  =  0/  X  r/  +  0/  X  Yj  (10.29d) 

Now,  since  vectors  r  j  are  fixed  to  the  moving  platform,  their  time-derivatives  are 
simply  given  by 


Yj=(o^Yj  (10.29e) 

On  the  other  hand,  vector  0/  is  directed  along  the  leg  axis,  and  so,  its  time-derivative 
is  given  by 


=  (o  j  X  ej 


with  0)  j  defined  as  the  angular  velocity  of  the  third  leg  link,  i.e., 

•  • 

=  (^1^1  +  02^2)  J 

the  subscript  J  of  the  above  parentheses  reminding  us  that  this  angular  velocity 
differs  from  leg  to  leg.  Clearly,  we  need  expressions  for  the  rates  of  the  first  two 
joints  of  each  leg.  Below  we  derive  the  corresponding  expressions.  In  order  to 
simplify  the  notation,  we  start  by  defining 

f/  =  (ci)/.  g/  =  (^2)/  (10.29f) 

Now  we  write  the  second  vector  equation  of  Eq.  (10.22a)  using  the  foregoing 
definitions,  which  yields 


X  ipj  +  ^4)^/  +  {02)jgj  X  {bj  +  b^)^j  +  bj^j  =  Cj 


where  b^  is  the  same  for  all  legs,  since  all  have  identical  architectures.  Now  we  can 
eliminate  (O2)  j  from  the  foregoing  equation  by  dot-multiplying  its  two  sides  by  g/, 
thereby  producing 

{d\)jgj  X  fy  •  {bj  +  Z)4)ey  +  gj'(eye5’)cy  =  g^Cy 

where  an  obvious  exchange  of  the  cross  and  the  dot  in  the  above  equation  has  taken 
place,  and  expression  (10.27a)  for  b j  has  been  recalled.  Now  it  is  a  simple  matter 
to  solve  for  (^i)  j  from  the  above  equation,  which  yields 


g^(l-eye^)cy 

Ay 


440 


10  Kinematics  of  Alternative  Robotic  Mechanical  Systems 


with  A  j  defined  as 


Ay  =  {bj  +  Z)4)ey  X  fy  •  gy  (10.30) 

Moreover,  we  can  obtain  the  above  expression  for  {0i)j  in  terms  of  the  platform 
twist  by  recalling  Eq.  (10.23),  which  is  reproduced  below  in  a  more  suitable  form 
for  quick  reference: 


Cy  =  Cyt 

where  t  is  the  twist  of  the  platform,  the  3x6  matrix  C y  being  defined  as 

Cy  =  [Ry  1] 

in  which  Ry  is  the  cross-product  matrix  of  ry  and  1  is  the  3x3  identity  matrix. 
Therefore,  the  expression  sought  for  (^i)  y  takes  the  form 

(0i)y  = -2-gy(l-eye5:)Cyt,  J  =1,11,..., VI  (10.31a) 

^y 

A  similar  procedure  can  be  followed  to  find  {O2)  j ,  the  final  result  being 

(02)y  =  ^fy(l-eye5:)Cyt,  J  =  1,11,..., VI  (10.31b) 

Ay 

thereby  completing  the  calculations  required  to  obtain  the  rates  of  all  unactuated 
joints.  Note  that  the  unit  vectors  involved  in  those  calculations,  ey,  fy,  and  gy ,  are 
computed  from  the  leg  inverse  kinematics,  as  discussed  above. 


Planar  and  Spherical  Manipulators 


The  velocity  analysis  of  the  planar  and  spherical  parallel  manipulators  of  Figs.  10.8 
and  10.9  are  outlined  below:  Using  the  results  of  Sect.  5.7.2,  the  velocity  relations 
of  the  /th  leg  of  the  planar  manipulator  take  the  form 

J^(9y=t,  /  =  /,//,///  (10.32) 


where  Jy  is  the  Jacobian  matrix  of  this  leg,  as  given  by  Eq.  (5.60),  while  ^  y  is  the 
three-dimensional  vector  of  joint  rates  of  this  leg,  i.e.. 


1  1  1 
Eryi  Ery2  Ery3 


-1 

'On 

,  0  J  = 

0j2 

- 

_^/3_ 

/  =  /,  //,  III 
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For  purposes  of  kinematic  velocity  control,  however,  we  are  interested  only  in 
the  first  joint  rate  of  each  leg;  i.e.,  all  we  need  to  determine  in  order  to  produce  a 

desired  twist  of  the  end-effector  is  not  all  of  the  foregoing  nine  joint  rates,  but  only 

•  •  • 

^//i,  and  Ojiii .  Thus,  we  want  to  eliminate  from  Eq.  (10.32)  the  unactuated  joint 

•  • 

rates  0j2  and  0/3,  which  can  be  readily  done  if  we  multiply  both  sides  of  the  said 
equation  by  a  three-dimensional  vector  n  j  perpendicular  to  the  second  and  the  third 
columns  of  J/ .  This  vector  can  be  most  easily  determined  as  the  cross  product  of 
those  two  columns,  namely,  as 


n  =  j/2  X  j/3 


r/2  —  r/3 


Upon  multiplication  of  both  sides  of  Eq.  (10.32)  by  n^,  we  obtain 

[-r5’2Ery3  +  (r/2  -  ry3)^Eryi]  Ojx  =  +  (r/2  -  ry3)^c  (10.33) 

and  hence,  we  can  solve  directly  for  Oji  from  the  foregoing  equation,  thereby 
deriving 


A  _  -(r^2Ery3)ft)  +  {rji  -  rjifc 
-r5’2Ery3  +  (r/2  -  r/3)^Eryi 

Note  that  Eq.  (10.33)  can  be  written  in  the  form 

jjdn  =  kj^t,  y  =  /,  //,  III 
with  jj  and  k/  defined,  for  J  =  I,  //,  III ,  as 


(10.34a) 


(10.34b) 


jj  =  (rji-rjiV'Erji 

=  [ry2Ery3  -(ry2  -  ry3)^]^ 


If  we  further  define 

0  =  ^Oji  djj2  0/7/3] 

and  assemble  all  three  foregoing  joint-rate-twist  relations,  we  obtain 


JO  =  Kt 


where  J  and  K  are  the  two  manipulator  Jacobians  defined  as 


J  =  diag(7/,  jii,  jiii),  K  = 


kf 

kf/ 

Lk[//, 


(10.35) 


(10.36) 


Expressions  for  the  joint  accelerations  can  be  readily  derived  by  differentiation  of 
the  foregoing  expressions  with  respect  to  time. 
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The  velocity  analysis  of  the  spherical  parallel  manipulator  of  Fig.  10.9  can  be 
accomplished  similarly.  Thus,  the  velocity  relations  of  the  /th  leg  take  on  the  form 

ijOj  =(o,  J  =  I,  II,  III  (10.37) 

where  the  Jacobian  of  the  /th  leg,  J/,  is  defined  as 

J/  =  [e/i  e/2  e/s] 

• 

while  the  joint-rate  vector  of  the  /th  leg,  ^/,  is  defined  exactly  as  in  the  planar  case 

analyzed  above.  Again,  for  kinematic  velocity  control  purposes,  we  are  interested 

•  •  • 

only  in  the  actuated  joint  rates,  namely,  Oji,  Om,  and  0///i.  As  in  the  planar  case, 

•  • 

we  can  eliminate  0j2  and  0/3  upon  multiplication  of  both  sides  of  Eq.  (10.37)  by 
a  vector  n /  perpendicular  to  the  second  and  the  third  columns  of  J/ .  An  obvious 
definition  of  this  vector  is,  then. 


n/  =  e/2  X  e/3 


The  desired  joint-rate  relation  is  thus  readily  derived  as 

jjOj^  =  kjcy,  J  =  I,  II,  III  (10.38) 

where  j j  and  k /  are  now  defined  as 

jj  =  e/1  X  e/2  •  e/3  (10.39a) 

k/  =  e/2  X  e/3  (10.39b) 

The  accelerations  of  the  actuated  joints  can  be  derived,  again,  by  differentiation  of 
the  foregoing  expressions. 

We  can  then  say  that  in  general,  parallel  manipulators,  as  opposed  to  serial  ones, 
have  two  Jacobian  matrices. 


10.3  Multifingered  Hands 

Shown  in  Fig.  10.10  is  a  three- fingered  hand  with  fingers  A,  B,  and  C,  each  supplied 
with  three  revolute  joints.  Furthermore,  each  finger  carries  two  revolutes  of  parallel 
axes  that  are  normal  to  the  axis  of  the  third  one.  Thus,  each  finger  comprises  three 
links,  the  one  closest  to  the  palm  V  being  of  virtually  zero  length  and  coupled  to  V 
via  a  revolute  joint.  Of  the  other  two,  that  in  contact  with  the  object  O  is  the  distal 
phalanx,  the  other  being  the  proximal  phalanx.  Moreover,  the  fingers  can  be  either 
hard  or  soft,  if  the  latter,  then  contact  takes  place  over  a  finite  area;  if  the  former,  then 
contact  takes  place  over  a  point,  and  hence,  hard  fingers  can  exert  only  force  and  no 
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Fig.  10.10  A  three- fingered  hand 


moment  on  the  manipulated  object.  Soft  fingers  can  exert  both  force  and  moment. 
For  the  sake  of  conciseness,  we  will  deal  only  with  hard  fingers  here.  Let  the  contact 
points  of  fingers  A,  jB,  and  C  with  O  be  denoted  hy  Aq,  Bq,  and  Co,  respectively. 
The  purpose  of  the  hand  is  to  manipulate  O  with  respect  to  V.  The  motion  of  O, 
moreover,  can  be  specified  through  its  pose,  given  in  turn  by  the  position  vector  o  of 
one  of  its  points,  O,  and  its  orientation  matrix  Q  with  respect  to  a  frame  fixed  to  V. 
Now,  in  order  to  manipulate  O  six  degrees  of  freedom  are  needed.  When  the  three 
fingers  are  in  contact  with  O,  the  hand-object  system  forms  a  parallel  manipulator 
with  three  “legs”  of  the  RRS  type,  with  S  standing  for  spherical  joint.  As  the  reader 
can  verify,  the  system  has  six-dof,  which  means  that  manipulations  are  possible  with 
only  two  actuated  revolutes  per  finger.  Many  designs  involve  only  two  motors  per 
finger,  one  of  the  revolute  joints  being  provided  with  springs  to  guarantee  contact. 

Thus,  the  location  of  the  three  contact  points  is  fully  determined  if  the  pose  of 
V  and  the  locations  oi  Aq,  Bq,  and  Co  in  O  are  given.  Once  the  position  vectors 
of  the  three  contact  points  are  known,  determining  the  joint- variable  values  needed 
to  take  O  to  the  desired  pose  reduces  to  solving  a  three-dimensional  positioning 
problem  for  each  finger,  with  three  revolute  joints — a  problem  already  discussed 
in  Sect.  4.4.1.  The  joint  rates  and  accelerations  are  then  determined  as  in  Sects.  4.4 
and  5.5. 

While  the  mechanics  of  grasping  is  quite  elaborate,  due  to  the  deformation  of 
both  fingers  and  object,  some  assumptions  will  be  introduced  here  to  produce  a 
simple  model.  One  such  assumption  is  rigidity;  a  second  is  smoothness,  under 
which  each  finger  is  capable  of  exerting  only  normal  force  on  the  object.  Moreover, 
this  force  is  unidirectional,  for  the  finger  cannot  exert  a  pull  on  the  object.  The 
smoothness  and  rigidity  assumptions  bring  about  limitations,  for  they  require  a 
rather  large  number  of  fingers  to  exert  an  arbitrary  wrench  on  the  grasped  object, 
as  shown  below. 
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Fig.  10.11  Geometry  of 
grasped  object  O 


We  assume  that  we  have  a  rigid  object  O  bounded  by  a  surface  S  that  is  smooth 
almost  everywhere,  i.e.,  it  has  a  well-defined  normal  n  everywhere  except  at  either 
isolated  points  or  isolated  curves  on  S.  Below  we  show  that  in  order  to  exert  an 
arbitrary  wrench  w  onto  O,  a  hand  with  rigid,  smooth  fingers  should  have  more 
than  six  fingers.  Assume  that  the  n  contact  points  on  S  are  {  Pf } ”  and  that  we  want 
to  find  n  pressure  values  {  A  / at  the  contact  points  that  will  produce  the  desired 
wrench  w  onto  O. 

Moreover,  let  the  unit  normal  at  Pf  be  denoted  by  and  the  vector  directed  from 
O  to  Pi  be  denoted  by  p/ ,  as  shown  in  Fig.  10.1 1. 

The  wrench  W/  exerted  by  each  finger  onto  O  at  Pi  is  apparently 


Wj  =  Xi 


p/  X  (-n,  ) 


9 


Xi  >0 


Upon  equating  the  resultant  wrench  with  the  desired  wrench,  we  obtain 


n 


1 


-Pz  X  n, 

-n, 


or  in  compact  form,  as 


GA  =  -w  (10.40a) 

where  G  is  the  6  x  grasping  matrix  and  A  is  the  w -dimensional  vector  of  pressure 
values,  i.e.. 


Pi  X  m  p„  X  n 


n 


ni 


n 


n 


(10.40b) 


Note  that  the  i  th  column  of  the  grasping  matrix  is  nothing  but  the  array  of  Pliicker 
coordinates  of  the  line  of  action  of  the  force  exerted  by  the  /  th  finger  on  the  object, 
in  ray  coordinates — see  Sect.  3.2.2. 
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Thus,  for  n  =  6,  3.  unique  pressure  vector  A  is  obtained  as  long  as  G  is 
nonsingular.  However,  negative  values  of  {A/}"  are  not  allowed,  and  since  nothing 
prevents  these  values  from  becoming  negative,  six  fingers  of  the  type  considered 
here  are  not  enough.  We  must  thus  have  more  than  six  such  fingers  in  order  to 
be  able  to  apply  an  arbitrary  wrench  onto  the  body.  For  n  >  6  and  a  full-rank 
6  xn  grasping  matrix,  nonnegative  values  of  {A  / can  be  generated,  but  only  under 
certain  conditions,  as  explained  below:  Let  u  be  a  vector  lying  in  the  null  space  of 
G,  i.e.,  such  that  Gu  =  0.  Then  an  arbitrary  A  can  be  expressed  as 

A  =  Aq  u 


where  Aq  is  a  particular  solution  of  Eq.  (10.40a).  For  example,  if  A o  is  chosen  as 
the  minimum-norm  solution  of  Eq.  (10.40a),  then  we  have,  explicitly, 

Aq  =  — G^W 

where  G^  is  the  generalized  inverse  of  G,  defined  as 

G"^  =  G^(GG^)“^ 


The  numerical  computation  of  the  minimum  norm  solution  of  an  undetermined 
system  of  linear  equations  is  discussed  in  Appendix  B. 

Note  that  the  6x6  product  GG^  has  the  general  form 


GG^  = 


Ei(Pi  X  “i)(Pi  X  n,  )^  Ei(P'  X  n,  )n,^ 


J' 


YU  nKP(  X  n,  ) 


T 


11/ n 


T 


Although  a  symbolic  expression  for  the  inverse  H  of  GG^  is  not  possible  in  the 
general  case,  we  can  always  express  this  inverse  in  block  form,  with  all  blocks  of 
3x3,  namely. 


H  =  (GG^)“^  = 

where  consistently,  Hu  has  units  of  meter“^,  H12  has  units  of  meter“\  and  H22  is 
dimensionless.  Moreover,  we  can  partition  G  into  two  xn  blocks,  i.e.. 


Hn  Hi2 
H?;  H22 


in  which  A  has  units  of  meter,  while  B  is  dimensionless.  Hence,  the  product  G^H 
takes  on  the  form 


G^H  =  [A^H„  +  B^H[2  A^Hi2  +  B^H22] 
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and  hence,  the  left-hand  block  of  the  foregoing  product  has  units  of  meter“\  while 
the  right-hand  block  is  dimensionless.  Upon  multiplying  the  desired  wrench  w  from 
the  left  by  this  product,  the  result,  Aq,  has  consistently  units  of  Newton. 

Now,  to  find  u,  several  numerical  methods  are  available  that  do  not  require 
any  matrix  inversion  (Golub  and  Van  Loan  1989).  A  simple  way  of  expressing  u, 
although  by  no  means  the  way  to  compute  it,  is  given  by 

u  =  Pv,  P  =  1-G^G 

where  v  is  an  -dimensional  vector  and  P  is  a  matrix  projecting  v  onto  the  null  space 
of  G,  and  1  defined  as  the  n  identity  matrix.  Now  we  are  left  with  the  task  of 
finding  v  so  that 


A;  —  Ui  ^0,  i  —  1,  . . . ,  n 


Hence,  our  policy  to  determine  u  is  simply,  for  /  =  1,  . . . ,  n, 


(  0,  if  Xoi  >  0; 
(  —Xoi ,  otherwise. 


Further,  v  is  found  upon  solving 


Pv  =  u 


However,  P  is  singular — its  rank  is  n  —  6,  as  the  reader  is  invited  to  prove — and  the 
above  equation  may  or  may  not  admit  a  solution.  For  a  solution  to  be  possible,  u 
must  lie  in  the  null  space  of  G. 

A  more  general  approach  to  solving  the  grasping  problem  relies  on  linear 
programming,  but  this  topic  lies  beyond  the  scope  of  the  book.  The  interested  reader 
is  directed  to  the  specialized  literature  on  the  subject  (Hillier  and  Lieberman  1995). 

In  the  presence  of  soft  fingers,  however,  fewer  than  six  fingers  suffice  to  grasp  an 
object.  Moreover,  in  the  presence  of  friction,  the  force  transmitted  by  a  finger  has, 
in  addition  to  its  normal  component,  a  tangential  component  that,  hence,  gives  rise 
to  a  contact  force  making  a  nonzero  angle  with  the  normal  to  the  object  surface  at 
the  i  th  contact  point.  Therefore,  by  virtue  of  the  linear  relation  between  the  normal 
and  the  tangential  components  of  the  transmitted  force,  given  by  the  coefficient  of 
friction  /x,  this  force  is  constrained  to  lie  within  thQ  friction  cone.  This  cone  has  its 
apex  at  the  contact  point  Pi ,  its  elements  making  an  angle  a  with  the  normal,  that 
is  given  by  o'  =  arctan(/x).  Furthermore,  by  virtue  of  the  fundamental  assumption 
of  Coulomb  friction  analysis,  /x  lies  between  0  and  1,  and  hence,  a  is  constrained  to 
lie  between  0°  and  45° . 

Shown  in  Fig.  10.12  is  an  example  of  a  three-fingered  hand.  This  hand  was 
developed  at  the  Katholieke  Universiteit  Leuven  (Van  Brussel  et  al.  1989). 


10.4  Walking  Machines 


447 


Fig.  10.12  A  prototype  of 
the  KU  Leuven  three-fingered 
hand  (courtesy  of  Prof.  H. 

Van  Brussel) 


The  literature  on  multifingered  hands  and  the  problem  of  grasping  is  far  richer 
than  we  can  afford  to  describe  here.  Extensive  studies  on  these  subjects  have  been 
reported  by  Reynaerts  (1995)  and  Teichmann  (1995). 


10.4  Walking  Machines 

Besides  the  walking  machines  introduced  in  Chap.  1,  namely,  the  OSU  Adaptive 
Suspension  Vehicle  and  the  TUM  Hexapod,  other  legged  machines  or  leg  designs 
are  emerging  with  special  features.  For  example,  CARL,  shown  in  Fig.  10.13, 
is  a  compliant  articulated  robot  leg  that  was  designed  at  McGill  University’s 
Centre  for  Intelligent  Machines  (CIM)  by  Prof.  Buehler  and  his  team  (Menitto  and 
Buehler  1996).  This  leg  contains  an  actuation  package  with  a  high  load-carrying 
capacity  (ATLAS)  and  an  antagonistic  pair  of  concentric  translational- to- angular 
displacement  devices.  The  leg  has  four  degrees  of  freedom,  of  which  two  are 
actuated  by  ATLAS  and  one  by  a  harmonic  drive  motor,  while  one  is  unactuated. 
This  leg  design  is  intended  to  provide  locomotion  to  a  quadruped. 

As  nature  shows  in  mammals,  four  legs  are  necessary  to  guarantee  the  static 
equilibrium  of  the  body  while  one  leg  is  in  the  swing  phase.  Static  equilibrium  is 
achieved  as  long  as  the  horizontal  projection  of  the  mass  center  of  the  overall  body- 
legs  system  lies  within  the  triangle  defined  by  the  contact  points  of  the  three  legs 
that  are  in  the  stance  phase.  More  than  four  legs  would  allow  for  greater  mobility. 
For  purposes  of  symmetry,  some  walking  machines  are  designed  as  hexapods,  so  as 
to  allow  for  an  equal  number  of  legs  in  the  swing  and  the  stance  phases. 
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Fig.  10.13  The  compliant 
articulated  robot  leg  (courtesy 
of  Prof.  M.  Buehler) 


Fig.  10.14  A  general  hexapod 


The  kinematic  analysis  of  walking  machines  is  possible  using  the  hexapod 
displayed  in  Fig.  10.14. 

Furthermore,  contact  with  the  ground  is  assumed  to  take  place  such  that  the 
ground  can  exert  only  a  “pushing”  force  on  each  leg  but  no  moment.  Thus,  while 
we  can  model  the  contact  between  leg  and  ground  as  a  spherical  joint,  care  must  be 
taken  so  that  no  pulls  of  the  ground  on  the  leg  are  required  for  a  given  gait. 
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Fig.  10.16  One  of  the  six 
identieal  legs  of  the  TU 
Munich  Hexapod  (courtesy  of 
Prof.  F.  Pfeiffer.  Reproduced 
with  permission  of  TSI 
Enterprises,  Inc.) 


Additionally,  we  shall  assume  that  the  leg  is  actuated  by  three  revolutes,  namely, 
those  with  variables  O4,  O5,  and  0^  in  Fig.  10.15,  where  Q  denotes  the  ground  and 
B  the  machine  body.  A  photograph  of  one  of  the  six  identical  legs  of  the  walking 
machine  developed  at  the  Technical  University  of  Munich,  introduced  in  Fig.  1 . 1 1 ,  is 
included  in  Fig.  10.16.  The  Denavit-Hartenberg  parameters  of  this  leg,  proceeding 
from  the  ground  upwards,  are  displayed  in  Table  10.3.  Note  that  the  architecture  of 
this  leg  is  simply  that  of  a  three-revolute  manipulator  carrying  a  spherical  joint  at 
its  end-effector,  similar  to  that  of  the  decoupled  manipulators  studied  in  Sect.  4.4. 
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Table  10.3  DH  parameters 
of  the  leg  of  the  TU-Munich 
walking  machine 


i 

Qi  (mm) 

bi  (mm) 

Oli 

1 

17 

0 

'O 

o 

o 

2 

123 

0 

o 

o 

oo 

3 

116 

0 

0° 

4 

0 

0 

o 

o 

5 

0 

0 

o 

o 

6 

0 

0 

0° 

The  spherical  joint  accounts  for  the  coupling  of  the  leg  with  the  ground.  We  are  thus 
assuming  that  when  a  leg  is  in  contact  with  the  ground,  the  contact  point  of  the  leg 
is  immobile.  At  the  same  time,  the  motion  of  the  body  B  is  prescribed  through  the 
motion  of  a  point  on  the  axis  of  the  revolute  coupled  to  the  body.  Such  a  point  is 
indicated  hy  Pj  for  the  Jth  leg.  Moreover,  the  point  of  the  Jth  leg  in  contact  with 
the  ground  will  be  denoted  hy  Oj.  Thus,  when  prescribing  the  motion  of  the  body 
through  that  of  each  of  the  six  points  Pj,  Pjj,  . . .,  Pyi ,  the  rigid-body  compatibility 
conditions  of  Eqs.  (8.14),  (8.15),  and  (8.28)  must  be  observed.  The  pose  of  the  body 
B  is  thus  specified  by  the  position  of  a  point  C  of  the  body  and  the  orientation 
matrix  Q  of  the  body  with  respect  to  a  frame  fixed  to  the  ground,  the  position  vector 
of  C  in  that  frame  being  denoted  by  c.  The  specification  of  points  Pj  to  Pyi  thus 
follows  from  the  knowledge  of  c  and  Q,  thereby  guaranteeing  compliance  with  the 
above-mentioned  constraints. 

Furthermore,  a  maneuver  of  S,  given  by  a  prescribed  pose,  can  be  achieved  by 
suitable  values  of  the  actuated-joint  variables,  which  thus  leads  to  a  problem  of 
parallel-manipulator  inverse  kinematics. 

The  mechanical  system  that  results  from  the  kinematic  coupling  of  the  machine 
legs  with  the  ground  is  thus  equivalent  to  a  parallel  manipulator.  The  essential 
difference  between  a  walking  machine  and  a  parallel  manipulator  is  that  the  former 
usually  involves  more  actuators  than  degrees  of  freedom.  This  feature  is  known  as 
redundant  actuation  and  will  not  be  pursued  here. 


10.5  Rolling  Robots 

Probably  the  rolling  robot  that  has  received  most  media  attention  is  NASA’s 
Sojourner,  of  the  Pathfinder  mission,  which  explored  a  spot  of  the  Martian 
landscape  for  several  months  in  1997.  It  is  noteworthy  that  the  Sojourner  was 
designed,  built,  and  commissioned  with  a  shoestring  budget  for  NASA  standards. 
The  Sojourner  is  a  paradigm  of  rolling  robots  for  autonomous  operation  on  rough 
terrain.  We  focus  here  on  the  simplest  robots  of  this  class,  i.e.,  robots  intended 
for  tasks  on  horizontal  surfaces,  and  so,  their  platforms  undergo  planar  motion, 
which  greatly  simplifies  their  kinematics.  One  special  feature  of  rolling  robots  is 
their  nonholonomic  nature.  What  this  means  is  that  the  minimum  number  m  of 
generalized  coordinates  defining  uniquely  a  posture  of  the  system  is  greater  than 
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Fig.  10.17  A  two-dof  rolling  robot:  (a)  its  general  layout;  and  (b)  a  detail  of  its  actuated  wheels 


the  number  n  of  their  independent  generalized  speeds,  i.e.,  m  >  n.  \n  the  case 
of  serial  and  parallel  manipulators,  paradigms  of  holonomic  systems,  m  =  n. 
In  nonholonomic  systems,  then,  we  must  distinguish  between  their  posture,  or 
configuration  degree  of  freedom  and  their  mobility,  or  velocity  degree  of  freedom. 
For  the  sake  of  conciseness,  we  will  refer  to  the  latter  whenever  we  mention  the 
degree  of  freedom  of  a  rolling  robot. 

Rolling  robots  are  basically  of  two  kinds,  depending  on  whether  they  are 
supplied  with  conventional  or  with  omnidirectional  wheels.  The  simplest  robots 
with  conventional  wheels  are  capable  only  of  two-dof  motions,  and  hence,  are 
kinematically  equivalent  to  conventional  terrestrial  vehicles.  However,  robots  with 
omnidirectional  wheels  (ODWs)  are  capable  of  three-dof  motions,  which  increases 
substantially  their  maneuverability.  Below  we  outline  the  kinematics  of  the  two 
kinds  of  robots. 


10.5.1  Robots  with  Conventional  Wheels 

We  begin  with  robots  rolling  on  conventional  wheels.  Since  these  have  two  degrees 
of  freedom,  they  need  only  two  actuators,  the  various  designs  available  varying 
essentially  in  where  these  actuators  are  located.  The  basic  architecture  of  this  kind 
of  robot  is  displayed  in  Fig.  10.17a,  in  which  we  distinguish  a  chassis,  or  robot  body, 
depicted  as  a  triangular  plate  in  that  figure:  two  coaxial  wheels  that  are  coupled  to 
the  chassis  by  means  of  revolutes  of  axes  passing  through  points  Oi  and  O2;  and  a 
third  wheel  mounted  on  a  bracket. 
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Now,  the  two  actuators  can  be  placed  in  two  essentially  different  arrays.  In  the 
first  array,  not  shown  in  the  figure,  one  actuator  is  used  for  propulsion  and  the  other 
for  steering,  the  former  being  used  to  provide  locomotion  power  to  the  common  two- 
wheel  axle  via  a  differential  gear  train.  This  train  is  required  to  allow  for  different 
angular  velocities  of  the  two  coaxial  wheels.  Moreover,  the  orientation  of  the  mid¬ 
plane  of  the  steering  wheel,  defined  by  angle  is  controlled  with  the  second 
actuator.  This  design  has  some  drawbacks,  namely,  (a)  the  two  motors  serving  two 
essentially  different  tasks  call  for  essentially  different  operational  characteristics, 
to  the  point  that  both  may  not  be  available  from  the  same  manufacturer;  (b)  the 
propulsion  motor  calls  for  velocity  control,  the  steering  motor  for  position  control, 
thereby  giving  rise  to  two  independent  control  systems  that  may  end  up  by  operating 
in  an  uncoordinated  fashion;  and  finally,  (c)  the  use  of  a  differential  gear  train 
increases  cost,  weight,  and  brings  about  the  inherent  backlash  of  gears. 

In  the  second  actuation  array,  shown  in  Fig.  10.17b,  the  two  coaxial  wheels 
are  powered  independently,  thereby  doing  away  with  the  differential  train  and 
its  undesirable  side  effects,  the  third  wheel  being  an  idle  caster.  Moreover,  the 
orientation  of  the  latter  is  determined  by  friction  and  constraint  forces,  thereby 
making  unnecessary  the  steering  control  system  of  the  first  array.  Below  we  analyze 
the  kinematics  of  a  robot  with  this  form  of  actuation. 

Let  point  C  of  the  platform  be  the  operation  point,  its  projection  onto  a  horizontal 
plane  l-L  containing  the  common  axis  of  the  two  actuated  wheels  being  C',  as 
indicated  in  Fig.  10.17b.  Let,  moreover,  the  position  vector  of  C'  in  a  frame  fixed 
to  the  ground,  with  origin  lying  in  H,  be  denoted  by  c.  Additionally,  let  co  be  the 
scalar  angular  velocity  of  the  platform  about  a  vertical  axis.  By  virtue  of  the  two-dof 
motion  of  this  robot,  we  can  control  either  the  velocity  c  of  C — or  of  C'  for  that 

matter — or  a  combination  of  co  and  a  scalar  function  of  c  by  properly  specifying  the 

•  • 

two  joint  rates  0i  and  O2.  However,  we  cannot  control  the  two  components  of  c  and 
CO  simultaneously. 

In  order  to  proceed  with  the  kinematic  analysis  of  the  system  at  hand,  we 
define  an  orthonormal  triad  of  vectors  whose  orientation  is  fixed  with  respect  to 
the  chassis.  Let  this  triad  be  denoted  by  {i,  j,  k},  with  k  pointing  in  the  upward 
vertical  direction.  Thus,  the  velocities  6/  of  points  Of,  for  i  =  1,2,  are  given  by 

6i=rdil  i  =  \,2  (10.41a) 

Furthermore,  the  velocity  of  C  can  now  be  written  in  two-dimensional  form  as 

c  =  6;  +  coE(c  —  Oi),  i  =  1,2  (10.41b) 

with  E  defined  as  in  Eq.  (5.55).  Thus,  all  vectors  of  Eq.  (10.41b)  are  two- 
dimensional.  Substituting  Eq.  (10.41a)  into  Eq.  (10.41b)  and  subtracting  sidewise 
Eq.  (10.41b)  for  i  =  1  and  for  i  =  2,  we  derive 


[r(0i  -d2)-col]j  =  O2 
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Hence,  the  angular  velocity  co  of  line  Oi  O2  in  planar  motion,  which  is  the  same  as 
that  of  the  platform,  can  be  readily  expressed  as 

^  =  j(0i-02)  (10.41c) 

its  positive  direction  being  that  of  k.  Upon  substitution  of  Eqs.  (10.41a  and  b)  into 
Eq.  (10.41b),  we  obtain  expressions  for  c  in  terms  of  the  joint  rates,  similar  to 
Eqs.  (10.41b),  for  i  =  1,2.  Eurthermore,  upon  adding  sidewise  the  two  expressions 
thus  resulting,  we  obtain  c  in  the  desired  form,  namely, 

t  =  aj(di-  62)1  +  ^(^1  +  4)j  (10.41d) 

Equations  (10.41c  and  d)  express  now  the  differential  direct  kinematics  relations 
of  the  robot  under  study.  In  compact  form,  these  relations  become 


t  =  L0a 


(10.41e) 


with  the  3x2  matrix  L  defined  as 


r/l  —r/l 

{arll)\  +  (r/2)j  -{ar/l)\  +  (r/2)j 


(10.41f) 


Moreover,  the  planar  twist  t  of  the  platform  and  the  two-dimensional  vector  ^  «  of 
actuated  joint  rates  are  defined  as 


0i 

Oi 


(10.41g) 


Computing  the  joint  rates  from  the  foregoing  equations,  i.e.,  solving  the  asso¬ 
ciated  inverse  kinematics  problem,  is  now  a  trivial  task.  The  inverse  kinematics 
relations  are  computed  below  by  noticing  that  Eq.  (10.41c)  provides  a  relation  for 
the  joint-rate  difference.  Thus,  all  we  need  now  is  a  second  equation  for  the  joint- 
rate  sum.  By  inspection  of  Eq.  (10.4  Id),  it  is  apparent  that  we  can  derive  this  relation 
by  dot-multiplying  both  sides  of  this  equation  by  j,  thereby  obtaining 

c.j=^(0i+02)  (10.42) 

The  two  equations  (10.41c)  and  (10.42)  can  now  be  cast  into  the  usual  form 


iOa  =  Kt 


(10.43a) 
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where  the  two  robot  Jacobians  J  and  K  are  given  below: 


(/  /  r)  0^ 

0  {2/r)f 


(10.43b) 


Note  that  J  is  a  2  x  2  matrix,  but  K  is  a  2  x  3  matrix. 

The  inverse  kinematics  relations  are  readily  derived  from  Eq.  (10.43a),  namely. 


Oi 

4 


1  (I  2 

2  7"  + 7^ 


1  //  2 

2  1 7"  “7^ 


where  y  =  c  •  j. 

Now,  in  order  to  complete  the  kinematic  analysis  of  the  robot  at  hand,  we 

•  • 

calculate  the  rates  of  the  unactuated  joints,  ^3  and  \[r.  To  this  end,  let  cof,  for 
i  =  1,2,3,  and  63  denote  the  three-dimensional  angular  velocity  vector  of  the  ith 
wheel  and  the  three-dimensional  velocity  vector  of  the  center  of  the  caster  wheel. 
Likewise,  0)4  denotes  the  scalar  angular  velocity  of  the  bracket. 

We  thus  have,  for  the  angular  velocity  vectors  of  the  two  actuated  wheels. 


(Oi  =  —Oil  +  cok  =  —Oil  j(0i  —  02)k 

1/ 


=  [-i  +  (r//)k  -(r//)k] 


'Oi 

07 


(O2  =  —021  +  (Ok  =  —021  +  t(^1  —  02)k 

%/ 


=  [('•/Ok  -i-  (r//)k] 


■01 ' 
02 


(10.44a) 


(10.44b) 


In  the  ensuing  derivations,  we  will  need  the  velocities  of  the  centers  of  the  two 
actuated  wheels,  which  were  derived  in  Eq.  (10.41a).  Moreover,  the  angular  velocity 
of  the  caster  wheel  can  be  readily  written  in  the  frame  fixed  to  the  bracket, 
{03,  f3,  k},  namely, 

0)3  =  ^303  +  (u)  +  ^)k  (10.45) 


with  \lr  denoting  the  angle  between  vectors  j  and  03  of  Fig.  10.17a,  measured  in  the 
positive  direction  of  k,  as  indicated  in  the  layout  of  Fig.  10.18. 

Note  that  vector  03  is  parallel  to  the  axis  of  rolling  of  the  caster  wheel,  while  f3  is 
a  horizontal  vector  perpendicular  to  03 .  These  two  sets  of  unit  vectors  are  related  by 


03  =  —  sin  \[ri  +  cos 
f3  =  —  cos  \lri  —  sin 


(10.46a) 

(10.46b) 
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Fig.  10.18  Layout  of  the  unit 
vectors  fixed  to  the  platform 
and  to  the  bracket 


their  inverse  relations  being 

i  =  —  sin  t/^e3  —  cos 
j  =  cos  —  sin 


(10.46c) 

(10.46d) 


Furthermore,  the  velocity  of  the  center  of  the  caster  wheel  is  derived  as 

63  =  a>3  X  rk  = 


while  the  scalar  angular  velocity  of  the  bracket,  0)4,  is  given  by 

(JO4  =  CO  ^  =  j(0i  —  O2)  (10.47) 

In  Chap.  12  we  shall  need  c  in  bracket  coordinates.  Such  an  expression  is  obtained 
from  Eqs.  (10.4 Id)  and  (10.46c  and  d),  namely, 

r  •  •  r  •  • 

c  =  -  O2)  sin  f  -(0 1  O2)  cos 

—[a-(di  —^2) cost//"  H — (^1  +  ^2)  sin t//']f3  (10.48) 

•  • 

Expressions  for  the  dependent  rates  in  terms  of  the  independent  ones,  0i  and  O2, 
are  readily  derived.  To  this  end,  we  express  the  velocity  of  P  in  two  independent 
forms,  one  in  terms  of  the  velocity  of  O3  and  the  other  in  terms  of  the  velocity  of 
C,  i.e.. 


p  =  63  +  C04k  X  (p  -  O3) 
p  =  C  +  U)k  X  (-Z^j) 


(10.49a) 

(10.49b) 
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Upon  equating  the  right-hand  sides  of  the  above  equations,  we  obtain  a  three- 
dimensional  vector  equation  relating  dependent  with  independent  rates,  namely, 

•  • 

— r^3f3  +  (u)  +  t/^)k  X  (p  —  03)  =  c  +  bco\ 

where  we  have  recalled  the  expressions  derived  above  for  63  and  0)4.  Further,  we 

•  • 

rewrite  the  foregoing  equation  with  the  unknown  rates,  ^3  and  xj/,  on  the  left-hand 
side,  i.e., 

—  rO^f^  +  X  (p  —  03)  =  c  +  bcoi  —  cok  x  (p  —  03)  (10.50) 

Moreover,  we  note  that,  from  Fig.  10.17, 

p  —  03  =  —df^  (h  —  r)k 


and  hence. 


k  X  (p  -03)  =  des 


Equation  (10.50)  thus  becoming 

—  rd2f3 xj/dei,  =  c (jo(bi  —  de^)  (10.51) 

•  • 

Now  it  is  a  simple  matter  to  solve  for  O3  and  xj/  from  Eq.  (10.51).  Indeed,  we  solve 
for  O3  by  dot-multiplying  both  sides  of  the  above  equation  by  f3 .  Likewise,  we  solve 
for  xj/  by  dot-multiplying  both  sides  of  the  same  equation  by  03 ,  thus  obtaining 

—  r^3  =  c  •  f3  +  cobi  •  f3 
dxjr  =  c  •  03  -h  co{b\  •  —  d) 


Now,  by  recalling  the  expressions  derived  above  for  co  and  c,  we  obtain 


c.f^  =  -aj(0i-  O2)  cos  -  ^(^1  +  O2)  sin  xj; 


—a  j  (di  —  62)  sinxlf  -  (Oi  +  62)  cos  xj/ 


C-03  = 

i  •  f3  =  —  cos  xj/,  i  •  03  =  —  sin 


Therefore, 


O3  =  a  cos  xl/(di  —  62)  +  2  +  ^2) 


^  =  p 


-(a  sin  +  5)(0i  -  4)  +  l(cos  if)(6i  +  4) 


(10.52a) 


(10.52b) 
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Fig.  10.19  (a)  The  Mekanum  Wheel;  (b)  its  side  view 


with  the  definitions  given  below: 


a  -\-  b  d 

a  =  —^,  S  =  j, 


P  = 


d 


(10.53) 


•  *  • 

Hence,  if  we  let  =  [^3  be  the  vector  of  unactuated  joint  rates,  then 

we  have 


eu  =  ^o 


a 


(10.54a) 


with  0  defined  as 


0  = 


a  cos  xj/  +  (sin  V^)/2  —a  cos  xj/  +  (sin  xf/)/! 

p[—a  sin  xjf  +  (cos  x[f)/2  —  8]  p[asmx[f  +  (cos  x[f)/2  8] 


(10.54b) 


thereby  completing  the  intended  kinematic  analysis. 


10.5.2  Robots  with  Omnidirectional  Wheels 

In  general,  omnidirectional  wheels  (ODWs)  allow  for  two  independent  translational 
motions  on  the  supporting  floor  and  one  independent  rotational  motion  about  a 
vertical  axis.  Based  on  the  shapes  of  the  wheels,  moreover,  ODWs  can  be  classified 
into  spherical  wheels  and  Mekanum  wheels,  the  latter  also  being  known  as  donators. 
Spherical  wheels  have  been  extensively  investigated  in  the  literature  (West  and 
Asada  1995).  We  focus  here  on  ODWs  of  the  Mekanum  type  and  assume  that  the 
robot  of  interest  is  equipped  with  n  of  these. 

The  Mekanum  wheel  bears  a  set  of  rollers  mounted  along  the  periphery  of  the 
wheel  hub  at  a  given  angle,  as  shown  in  Fig.  10.19a.  Furthermore,  the  rollers  are 
shaped  so  that  the  wheel  appears  as  circular  on  its  side  view,  as  shown  in  Fig.  10. 19b, 
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Fig.  10.20  The  active  roller 
of  the  i  th  wheel 


\ 


in  order  to  ensure  a  smooth  motion.  Pairwise  orthogonal  unit  vectors  ,  f,  and  , 
h;  are  defined  on  the  middle  horizontal  planes  of  the  wheel  hub  and  of  the  roller 
in  contact  with  the  floor,  respectively.  This  roller  is  termed  active  in  the  discussion 
below.  Now  we  aim  at  finding  the  kinematic  relation  between  the  wheel  joint  rates 
{  Oi  }\  and  the  Cartesian  velocity  variables  of  the  robot,  namely,  the  scalar  angular 
velocity  co  and  the  two-dimensional  velocity  vector  c  of  the  platform  centroid.  To 
this  end,  we  express  the  velocity  6,  of  the  centroid  Oi  of  the  /th  wheel  in  two 
different  forms:  first  we  look  at  this  velocity  from  the  active  roller  up  to  the  centroid 
Oi ;  then,  from  the  platform  centroid  C  to  Oi . 

If  we  relate  the  velocity  of  Oi  with  that  of  the  contact  point  of  the  active  roller 
with  the  ground,  then  we  can  write,  with  the  aid  of  Fig.  10.20, 

Oz  =  Pi  +  V,-  (10.55) 

with  V/  defined  as  the  relative  velocity  of  Oi  with  respect  to  Pi .  Now  let  (Oh  and  cOr 
denote  the  angular- velocity  vectors  of  the  hub  and  the  roller,  respectively,  i.e., 

•  • 

Wh  =  a)k  +  Oi^i ,  (dr  =  (Oh+  (pigi 

We  thus  have 

- y 

Pi  =  (Or  X  QiPi  =  (ftik  +  fte,-  +  (pigi)  X  bk 

where  b  is  the  radius  of  the  rollers  at  the  contact  point  with  ground.  In  addition,  Oi 
denotes  the  rate  of  the  wheel  hub,  while  (pi  denotes  that  of  the  active  roller,  which 
are  positive  in  the  directions  of  vectors  and  g, ,  respectively.  Hence, 


Vi  = 


biOifi  +  (pihi) 


(10.56) 
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Fig.  10.21  The  layout  of  the 
i  th  wheel  with  respect  to  the 
robot  platform 


Moreover, 


V;  =  (Oh  X  Pj  Oi  =  (ft)k  +  9i^i)  x{a-  b)k 
a  denoting  the  height  of  the  axis  of  the  wheel  hub,  as  shown  in  Fig.  10.19b.  Thus, 

V,-  =  -di(a-b)fi  (10.57) 

thereby  obtaining  the  desired  expression  for  6/ ,  namely, 

6;  =  -aOiti  -b^ihi  (10.58) 

A  general  layout  of  the  /th  ODW  with  roller  axes  at  an  angle  at  with  respect 
to  the  normal  e,  to  the  middle  vertical  plane  of  the  corresponding  hub  is  shown  in 
Fig.  10.21.  The  subscript  /  is  associated  with  both  the  /  th  wheel  and  its  active  roller. 
Moreover,  the  velocity  6/  of  the  /  th  wheel  can  be  expressed  in  terms  of  the  Cartesian 
velocity  variables,  c  and  co,  as 


6,  =  c  +  u)Ed,  (10.59) 

where  we  have  used  a  two-dimensional  vector  representation,  with  d,  defined  as 
the  vector  directed  from  point  C  to  the  centroid  Oi  of  the  hub  and  E  defined  as 
in  Eq.  (5.55).  Furthermore,  since  all  rollers  are  unactuated  and  they  rotate  idly,  the 
value  of  0,  is  immaterial  to  our  study.  Hence,  we  eliminate  this  variable  from  the 
foregoing  equations,  which  is  done  by  dot-multiplying  both  sides  of  Eq.  (10.58)  by 
g; ,  normal  to  h/ ,  thereby  deriving 

gfo,-  =  -aOigJii 


But 
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Therefore, 


gfo,  =  -a(smoii)9i  (10.60) 

The  same  multiplication  performed  on  Eq.  (10.59)  yields 

gf  =  (gf  Ed,)«  +  g[  C  (10.61) 

Upon  equating  the  right-hand  sides  of  Eqs.  (10.60)  and  (10.61),  we  derive  the 
desired  relation,  namely. 


•  /y-r 

—  a(sinai)0i  =  k-  t,  i  =  I, ...  ,n  (10.62) 

where  the  three-dimensional  vector  k/  is  defined  as 

k, = [  1 

L  J 

and  the  twist  vector  t  is  as  defined  in  Eq.  (10.41g).  We  now  define  the  vector  of 
wheel  rates  0  in  the  form 


o^[e,e2--e„f 


(10.63) 


If  the  n  equations  of  Eq.  (10.62)  are  now  assembled,  we  obtain 

JO  =  Kt  (10.64) 

where,  if  we  assume  that  all  angles  at  are  identical  and  labeled  a,  then  the  n  x  n 
Jacobian  J  and  the  n  x3  Jacobian  K  take  the  forms 


J  =  —a  sin  O' 1 


g[Edi  g[ 


(10.65a) 

(10.65b) 


with  1  denoting  the  n  xn  identity  matrix. 

Given  Eqs.  (10.65a)  and  (10.65b),  the  differential  inverse  kinematics  can  be 
resolved  as 


1 

0  = - ^ 


a  sina 


(10.66) 
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whence  it  is  apparent  that  sin  o'  must  be  different  from  zero,  i.e.,  the  axes  of  the 
rollers  must  not  be  parallel  to  the  axis  of  the  hub.  If  these  axes  are  parallel,  then  the 
ODWs  reduce  to  conventional  wheels. 


10.6  Exercises 


10.1  For  the  parallel  manipulator  of  Fig.  10.2,  find  the  matrix  mapping  joint  forces 
into  wrenches  acting  on  the  moving  platform,  if  actuation  is  supplied  through 
the  prismatic  joints. 

10.2  Show  that,  if  det(H)  of  Eq.  (10.21)  is  expanded  in  the  form 

det(H)  =  Hi  Ai  -  H2A2  +  Hs  As 


then  Ai,  A2,  and  A3  are  14th-,  13th-,  and  12th-degree  polynomials  in  r2, 
respectively. 

10.3  What  is  the  counterpart  of  a  decoupled  serial  manipulator,  as  described 
in  Sect.  4.4,  of  a  six-dof  parallel  manipulator  with  an  architecture  similar 
to  that  of  Fig.  10.2?  What  is  the  degree  of  the  characteristic  polynomial 
of  that  parallel  manipulator?  Compare  this  answer  with  the  characteristic 
polynomial  derived  in  Sect.  4.4. 

10.4  We  refer  to  the  rolling  robot  with  conventional  wheels  introduced  in 
Sect.  10.5.1.  We  would  like  to  study  the  equivalent  concept  of  manipulability, 
which  here  we  can  call  maneuverability.  This  concept  refers  to  the  numerical 
conditioning  of  the  two  underlying  Jacobian  matrices,  J  and  K,  as  defined  in 
Eqs.  (10.43a  and  b).  Clearly,  J  is  isotropic  and  hence,  optimally  conditioned. 
In  attempting  to  determine  the  condition  number  of  K,  however,  we  need  to 
order  its  singular  values  from  smallest  to  largest. 

(a)  Show  that  the  two  singular  values  of  K  are  cti  =  I /r  and  02  =  2/r. 
Obviously,  an  ordering  from  smallest  to  largest  is  impossible  because  of 
the  lack  of  dimensional  homogeneity. 

(b)  In  order  to  cope  with  the  dimensional  inhomogeneity  of  matrix  K,  we 
introduce  the  characteristic  length  L,  which  we  define  below.  First,  we 
redefine  the  Jacobian  K  in  dimensionless  form  as 


K 


(//r)  0  0 

0  0  2L/r 


Now,  L  is  the  value  that  minimizes  the  condition  number  of  the 
dimensionless  K.  Show  that  this  value  is  //2  and  that  it  produces  a 
condition  number  of  unity. 
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10.5  With  reference  to  the  robot  of  Fig  10.17,  assume  that  the  motors  are 
placed  rather  on  the  joints  associated  with  variables  \l/  and  O3.  Under  these 
conditions, 

•  •  •  • 

(a)  Find  the  matrix  ^  mapping  array  [^3  into  array  [0i  02]^- 

(b)  Is  it  possible  to  find  values  of  \[r,  a,  8,  and  p  that  will  render  ^  isotropic? 

If  so,  which  are  these  values? 

10.6  Find  an  expression  for  the  angular  velocity  (pi  of  the  active  roller  of  the  ith 
wheel  of  the  robot  with  Mekanum  wheels  introduced  in  Sect.  10.5.2. 

10.7  We  refer  again  to  the  robot  with  Mekanum  wheels  introduced  in  Sect.  10.5.2. 
For  the  case  of  a  three-wheeled  robot  of  this  kind,  we  consider  here  a 
design  whereby  the  wheels  are  equally  spaced  in  a  A -array.  In  this  array, 
the  centers  of  the  hubs.  Of ,  lie  at  the  corners  of  an  equilateral  triangle  of 
side  a;  moreover,  we  assume  that  =  90°,  for  /  =  1,  2,  3.  Under  these 
conditions,  find  the  characteristic  length  L  of  the  robot  that  renders  K,  as 
defined  in  the  above-mentioned  subsection,  dimensionless  and  of  a  minimum 
condition  number.  Find  this  minimum  as  well. 

10.8  Find  the  value  of  xj/  at  which  the  rolling  robot  of  Fig.  10.17  attains  a  singular 
configuration.  Here,  a  singularity  is  understood  as  a  loss  of  maneuverability 
in  the  sense  of  not  being  able  to  drive  the  unactuated  joints  by  means  of 
the  actuated  ones.  Discuss  whether  under  reasonable  values  of  the  geometric 
parameters,  this  singularity  can  occur. 

10.9  Determine  the  architecture  and  the  “posture”,  i.e.,  the  values  of  the  relevant 
joint  variables  of  the  rolling  robot  of  Fig.  10.17  that  will  render  matrix 
0  isotropic,  where  0  represents  the  mapping  of  actuated  joint  rates  into 
unactuated  ones.  Is  kinematic  isotropy,  in  this  sense,  kinematically  possible? 

10. 10  Find  a  relation  among  the  geometric  parameters  of  the  robot  of  Fig.  10. 17  that 
will  allow  the  steering  of  the  robot  along  a  straight  course  with  the  highest 
possible  maneuverability  in  the  sense  defined  in  Exercise  10.8.  That  is,  find  a 
relation  among  the  geometric  parameters  of  this  robot  that  will  render  /c(0) 
a  minimum  along  a  straight  course. 

10. 1 1  Find  the  value  of  xp  under  which  the  robot  of  Fig.  10. 17  performs  a  maneuver 
that  leaves  the  midpoint  of  segment  Oi  O2  stationary.  Under  this  maneuver, 
state  a  relationship  among  the  geometric  parameters  of  the  robot  that 
minimizes  k  (0). 

10.12  Upon  inversion,  Eq.  (10.54a)  yields 


(a)  FindU. 

(b)  The  above  equation  can  be  written  as 

•  •  •  rj~i  • 

61  =  Mi303  +  =  Uj  9u 

•  •  •  rr<  • 

^2  =  ^23  ^3  +  U2f  Xj/  =  U2  ^  M 
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The  first  of  the  above  equations  can  be  integrated  if  ui,  which  is  an 
implicit  function  of  O3  and  \l/,  is  the  gradient  with  respect  to  Ou  = 
[O2  \[f]^  of  3,  scalar  function  1/1(63,  \[f).  Likewise,  the  second  of  the 

above  equations  can  be  integrated  if  a  function  1/2(63,  xj/)  exists,  whose 
gradient  with  respect  to  Ou  is  U2.  Further,  upon  recalling  Schwartz’s 
Theorem  of  multivariable  calculus,  U/  is  such  a  gradient  if  and  only  if 
Vu;,  i.e.,  the  Hessian  matrix  of  Ut  with  respect  to  is  symmetric,  for 
i  =  1,2. 

Show  that  the  above-mentioned  Hessians,  for  the  case  at  hand,  are 
nonsymmetric,  and  hence,  none  of  the  above  differential  expressions  is 
integrable.  Such  expressions  are  called  nonholonomic . 

Note:  To  be  sure,  the  above  condition  is  sufficient,  but  not  necessary. 
It  is  possible  that  some  individual  equations  of  a  system  of  differential 
expressions,  also  called  Pfaffian  forms,  are  not  integrable  while  the 
overall  system  is.  An  examination  of  necessary  and  sufficient  conditions 
for  integrability  falls  beyond  the  scope  of  this  book.  Such  conditions  are 
best  understood  with  the  aid  of  the  Frobenius  Theorem  (De  Luca  and 
Oriolo  1995)  and  its  analog,  the  Holonomy  Theorem  (Ostrovskaya  and 
Angeles  1998). 

10.13  For  the  rolling  robot  with  omnidirectional  wheels  introduced  in  Sect.  10.5.2, 
with  a  A -array,  as  described  in  Exercise  10.7,  show  that  the  equation  yielding 
the  angular  velocity  of  the  platform  in  terms  of  the  wheel  rates  is  integrable, 
but  the  equations  yielding  the  velocity  of  the  operation  point  are  not. 

10.14  A  holonomic  rolling  robot.  The  robot  described  in  Exercise  10.13  can  be 
rendered  holonomic  at  the  expense  of  one  degree  of  freedom.  Show  that  if 
the  three  wheel  rates  are  coordinated,  either  mechanically  or  electronically 
so  that 


61  ~\~  62  ~\~  63  —  0 


then  the  platform  is  constrained  to  move  under  pure  translation.  When 
operating  in  this  mode,  the  robot  is  holonomic.  Find  an  explicit  expression 
for  the  position  vector  c  of  the  operation  point  in  terms  of  the  wheel  angles. 


Chapter  11 

Trajectory  Planning:  Continuous-Path 
Operations 


11.1  Introduction 

As  a  follow-up  to  Chap.  6,  where  we  studied  trajectory  planning  for  pick-and-place 
operations  (PPO),  we  study  in  this  chapter  continuous-path  operations.  In  PPO, 
the  pose,  twist,  and  twist-rate  of  the  EE  are  specified  only  at  the  two  ends  of 
the  trajectory,  the  purpose  of  trajectory  planning  then  being  to  blend  the  two  end 
poses  with  a  smooth  motion.  When  this  blending  is  done  in  the  joint- variable  space, 
the  problem  is  straightforward,  as  demonstrated  in  Chap.  6.  There  are  instances  in 
which  the  blending  must  be  made  in  Cartesian  space,  in  which  advanced  notions 
of  interpolation  in  what  is  known  as  the  image  space  of  spatial  displacements,  as 
introduced  by  Ravani  and  Roth  (1984),  are  needed.  The  image  space  of  spatial 
displacements  is  a  projective  space  with  three  dual  dimensions,  which  means  that  a 
point  of  this  space  is  specified  by  four  coordinates — similar  to  the  homogeneous 
coordinates  introduced  in  Sect.  2.5 — of  the  form  Xi  +  for  /  =  1,2,  3, 4, 
where  e  is  the  dual  unity,  which  has  the  property  that  =  0.  The  foregoing 
coordinates  are  thus  dual  numbers,  their  purpose  being  to  represent  both  rotation 
and  translation  in  one  single  quantity.  In  following  Ravani  and  Roth’s  work,  Ge  and 
Kang  (1995)  proposed  an  interpolation  scheme  that  produces  curves  in  the  image 
space  with  second-order  geometric  continuity,  which  are  referred  to  as  curves. 
These  interpolation  techniques  lie  beyond  the  scope  of  the  book  and  will  be  left 
aside.  The  interested  reader  will  find  a  comprehensive  and  up-to-date  review  of  these 
techniques  in  (Srinivasan  and  Ge  1997). 

The  purpose  of  this  chapter  is  to  develop  motion  interpolation  techniques  in 
Cartesian  space  that  produce  smooth  motions  in  both  Cartesian  and  joint  spaces. 
Motion  interpolation  in  joint  space  was  discussed  in  Chap.  6,  the  present  chapter 
being  devoted  to  motion  interpolation  in  Cartesian  space.  To  this  end,  we  resort  to 
basic  notions  of  differential  geometry. 
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1 1  Trajectory  Planning:  Continuous-Path  Operations 


11.2  Curve  Geometry 

Continuous-path  robotics  applications  appear  in  operations  such  as  arc-welding, 
flame-cutting,  deburring,  and  routing.  In  these  operations,  a  tool  is  rigidly  attached 
to  the  end-effector  of  a  robotic  manipulator,  the  tool  being  meant  to  trace  a 
continuous  and  smooth  trajectory  in  a  six-dimensional  conflguration  space.  Three 
dimensions  of  this  space  describe  the  spatial  path  followed  by  the  operation  point 
of  the  EE,  while  the  remaining  three  describe  the  orientation  of  the  EE.  Some 
applications  require  that  this  task  take  place  along  a  warped  curve,  such  as  those 
encountered  at  the  intersections  of  warped  surfaces,  e.g.,  in  aircraft  fuselages, 
while  the  path  is  to  be  traversed  as  a  prescribed  function  of  time.  This  function, 
moreover,  is  task-dependent;  e.g.,  in  arc-welding,  the  electrode  must  traverse  the 
path  at  a  constant  speed,  if  no  compensation  for  gravity  is  taken  into  account. 
If  gravity  compensation  is  warranted,  then  the  speed  varies  with  the  orientation  of 
the  path  with  respect  to  the  vertical.  Below  we  will  deflne  this  orientation  as  that  of 
the  Frenet-Serret  frame  associated  with  every  point  of  the  path  where  the  path  is 
smooth. 

Moreover,  for  functional  reasons,  the  orientation  of  the  EE  is  given  as  a  rotation 
matrix  that  is,  in  turn,  a  prescribed  smooth  function  of  time.  In  arc-welding,  for 
example,  the  orientation  of  the  electrode  with  respect  to  the  curve  must  be  constant. 
The  trajectory  planning  of  the  conflguration  subspace  associated  with  the  warped 
path  is  more  or  less  straightforward,  but  the  planning  of  the  trajectory  associated 
with  the  orientation  subspace  is  less  so. 

While  most  methods  of  trajectory  planning  at  the  Cartesian-coordinate  level 
focus  on  the  path  followed  by  the  operation  point,  the  underlying  inverse  kinematics 
of  a  six-axis  robotic  manipulator  requires  the  speciflcation  of  the  orientation  of  the 
EE  as  well.  In  the  presence  of  simple  manipulators  with  a  spherical  wrist,  as  those 
studied  in  Sect.  4.4.2,  the  positioning  and  the  orientation  tasks  are  readily  separable, 
and  hence,  the  planning  of  the  two  tasks  can  be  done  one  at  a  time.  In  other  instances, 
e.g.,  in  most  arc-welding  robots,  such  a  separation  is  not  possible,  and  both  tasks 
must  be  planned  concurrently,  which  is  the  focus  of  our  discussion  below.  Here,  we 
follow  the  technique  presented  in  Angeles  et  al.  (1988). 

Crucial  to  our  discussion  is  the  concept  of  path  orientation.  Let  F  be  a  warped 
curve  in  three-dimensional  space  that  is  smooth  in  a  certain  interval  of  interest 
for  our  discussion.  Under  these  conditions,  we  can  associate  with  every  point  of 
this  interval  an  orthonormal  triad  of  vectors,  i.e.,  a  set  of  unit  vectors  that  are 
mutually  orthogonal,  namely,  the  tangent,  the  normal,  and  the  binormal  vectors 
of  r .  Therefore,  when  this  set  of  vectors  is  properly  arranged  in  a  3  x  3  array, 
a  rotation  matrix  is  obtained.  This  matrix  thus  represents  the  orientation  of  F . 
In  order  to  parameterize  these  vectors,  let  ^  be  the  arc  length  measured  along  F 
from  a  certain  reference  point  on  this  curve.  Below  we  review  the  basic  differential- 
geometric  concepts  pertaining  to  our  discussion. 

The  tangent,  normal,  and  binormal  unit  vectors,  e^,  e„,  and  e^,  respectively, 
associated  with  every  point  of  F  where  this  curve  is  smooth,  are  generically  termed 
here  the  Frenet-Serret  vectors.  These  vectors  are  defined  as 
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e?  =  r' 

(11.1a) 

r'  X  y" 

e*  —  1  ,  ,,, 

(11.1b) 

1  y'  X  y"  \ 

^ 

(11.1c) 

where  r'  stands  for  drlds  and  r"  for  d^rjds^.  Now  the  Frenet-Serret  relations 
among  the  three  foregoing  unit  vectors  and  the  curvature  k  and  torsion  r  of  F  are 
recalled  (Brand  1965): 


det 

ds 

=  K^n 

(11.2a) 

d  ^Yi 

ds 

=  -K&t  +  T:^b 

(11.2b) 

d^h 

ds 

=  -r^n 

(11.2c) 

Moreover,  the  curvature  and  torsion  can  be  calculated  with  the  aid  of  the  formulas 


K 

X 


y'  X  y" 


y'  X  y"  •  y'" 


(11.3a) 

(11.3b) 


where  y'"  stands  for  d^Yjds^ .  Furthermore,  differentiation  of  k  and  r,  as  given 
above,  with  respect  to  yields 


k'{s)  =  {y'  X  r") .  ^ - -  (11.4a) 

K 

r'  X  y"  •  —  2r(r'  x  r'')  •  (F  x  r'") 

r'(s)  =  - ^  (11.4b) 

where  stands  for  d^Y/ds^.  The  geometric  interpretation  of  the  curvature  is  the 
rate  of  change  of  orientation  of  the  tangent  vector  with  respect  to  the  arc  length;  that 
of  the  torsion  is  the  rate  at  which  the  curve  quits  the  plane  of  the  tangent  and  normal 
vectors.  Thus,  at  points  where  the  curvature  vanishes,  the  curve  approximates  a  line 
to  a  second  order,  i.e.,  up  to  second-order  derivatives,  whereas  at  points  where  the 
torsion  vanishes,  the  curve  approximates  a  planar  curve  to  a  third  order — Notice  that 
the  torsion  involves  third-order  derivatives.  Now,  from  the  Frenet-Serret  formulas 
and  the  chain  rule,  we  can  derive  the  time-rate  of  change  of  the  Frenet-Serret 
vectors,  namely, 

e„  =  =  -SK^t  +  ST:^b 

h  =  =  -srcn 


(11.5a) 

(11.5b) 

(11.5c) 
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Furthermore,  let  (o  be  the  angular  velocity  of  the  Frenet-Serret  frame.  Then,  clearly. 


et  =  (0  xet 

(11.6a) 

0^  =  0)  X  0^ 

(11.6b) 

et  =  (o  xet 

(11.6c) 

Upon  equating  pairwise  the  right-hand  sides  of  Eqs.  (1 1.5a-c)  and  Eqs.  (1 1.6a-c), 
we  obtain  three  vector  equations  determining  (o ,  namely. 


—  Et(o=SKen  (11.7a) 

—Eno)  =  —SKet  +  ire^  (11.7b) 

—Ebo)  =  — ire„  (11.7c) 

where  we  have  introduced  the  cross-product  matrices  Ec  E„ ,  and  Eb  of  vectors  , 
e„ ,  and  ,  respectively,  thereby  obtaining  a  system  of  nine  scalar  equations  in  the 
three  unknown  components  of  o),  i.e., 

A(o=h  (11.8a) 

with  A  defined  as  the  9x3  matrix  and  b  as  the  nine-dimensional  vector  displayed 
below: 


Et 

SKGn 

A  =  - 

En 

,  b  = 

s(-Ket  +  ret) 

_Eb_ 

-sren 

(11.8b) 


Although  the  foregoing  system  is  overdetermined,  it  is  consistent,  and  hence  it 
comprises  exactly  three  linearly  independent  equations,  the  remaining  six  being 
dependent  on  the  former.  One  way  to  reduce  system  (11.8a)  to  only  three  equations 
consists  in  multiplying  both  sides  of  this  equation  by  A^.  Now,  the  product  A^A 
greatly  simplifies  because  matrix  A  turns  out  to  be  isotropic,  as  per  the  discussion 
of  Sect.  5.8,  i.e.,  its  three  columns  are  mutually  orthogonal  and  all  have  the  same 
magnitude.  This  fact  can  become  apparent  if  we  realize  that  the  three  3x3  blocks 
of  A  are  cross-product  matrices  of  three  orthonormal  vectors.  Thus, 


A^A  =  EfE, +E„X+E[E^ 


If  we  now  recall  Theorem  2.3.4,  the  foregoing  products  take  on  quite  simple  forms, 
namely. 
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Moreover,  for  any  three-dimensional  vector  v,  we  have 

(e,ef  +  e„ej  +  eie[)v  =  v 

and  hence,  the  above  sum  in  parentheses  reduces  to  the  identity  matrix,  i.e., 

e,ef  +  e„el  +  e*e[  =  1 
the  product  A  thus  reducing  to 


A^A  =  (2)1 


Therefore,  (o  takes  on  the  form 

SKtn 

s(-Ket  +  ret) 
-ire„ 


0)  =  -  [E^  E/7  EZ?] 


or  upon  expansion, 

«  =  -  [kg,  X  e„  +  e„  X  (re^,  -  /ce,)  -  re^,  x  e„]  (11.10) 

However,  since  the  Frenet-Serret  triad  is  orthonormal,  we  have 

e^xe„=e^,  e„xe^=e^,  x  =  e„  (11.11) 

Upon  substitution  of  expressions  (11.11)  into  the  expression  for  (o  given  in 
Eq.  (11.10),  we  obtain 

(0=s8  (11.12) 

where  8  is  the  Darboux  vector,  defined  as 


5  =  re^  +  K^h  (11.13) 

Expressions  for  the  curvature  and  torsion  in  terms  of  the  time-derivatives  of  the 
position  vector  are  readily  derived  using  the  chain  rule,  which  leads  to 


K 


r 


r  X  r 


r| 

r  X  r-  r 


•  •  •  I 

r  X  rM 


(11.14a) 

(11.14b) 
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Upon  differentiation  of  both  sides  of  Eq.  (11.12),  the  angular  acceleration  cb  is 
derived  as 


cb=sS-hsS  (11.15) 

where  the  time-derivative  of  the  Darboux  vector  is  given,  in  turn,  as 

S  =  ref  (11.16) 

in  which  Eqs.  (11.5a-c)  have  contributed  to  the  simplification  of  the  above 
expression.  The  time-derivatives  of  the  curvature  and  torsion  are  readily  derived 
by  application  of  the  chain  rule,  thereby  obtaining 

k  =  sk'(s)  =  -(U  X  r'")  •  (U  X  r")  (11.17a) 

K 

r  =  sr'(s)  =  X  r"  •  —  2r(r'  x  r'")  •  (r'  x  r")]  (11.17b) 

The  time-derivative  of  the  Darboux  vector  thus  reduces  to 


8  =  s(Aet  +  Bet) 

where  scalars  A  and  B  are  computed  as 

r'  X  r"  •  —  2r(r'  x  r'")  •  (r'  x  U') 

^  ^ - 

(U  X  r'")  •  (U  X  r") 


K 

and  hence,  the  angular  acceleration  reduces  to 

CO  =  ^‘5  +  s^(Aet  +  Bet) 


(11.18a) 


(11.18b) 

(11.18c) 


(11.19) 


Erom  the  relations  derived  above,  it  is  apparent  that  the  angular  velocity  is  a 
bilinear  function  of  the  Darboux  vector  and  i,  while  the  angular  acceleration  is 
linear  in  s  and  quadratic  in  i.  The  computational  costs  involved  in  the  calculation 
of  the  angular  velocity  and  its  time-derivative  amount  to  31  multiplications  and 
13  additions  for  the  former,  and  28  multiplications  with  14  additions  for  the  latter 
(Angeles  et  al.  1988).  Notice  that  the  angular  velocity  requires,  additionally,  one 
square  root. 

In  the  above  discussion,  it  is  assumed  that  explicit  formulas  for  the  two  time- 
derivatives  of  the  arc  length  ^  are  available.  This  is  often  not  the  case,  as  we  show 
with  the  examples  below,  whereby  an  intermediate  parameter,  which  is  easier  to 
handle,  is  introduced.  What  we  will  need  are,  in  fact,  alternative  expressions  for  the 
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quantities  involved,  in  terms  of  kinematic  variables;  i.e.,  we  need  time-derivatives 
of  the  position  vector  r  rather  than  derivatives  of  this  vector  with  respect  to  the  arc 
length  Below  we  derive  these  expressions. 

First,  note  that  can  be  obtained  by  simply  normalizing  the  velocity  vector  r, 
namely,  as 


where  it  is  not  difficult  to  realize  that 


s 


(11.20) 


(11.21) 


Moreover,  the  binormal  vector  can  be  derived  by  application  of  the  chain  rule  to 
vector  r',  namely. 


But 


and  hence. 


dr'  _  dr' I dt 
ds  ds/dt 


1 

s 


d 

dt 


sr  —  sr 


(11.22a) 


(11.22b) 


(11.22c) 


Now,  upon  substitution  of  expressions  (1 1.22b  and  c)  into  Eq.  (1 1.1b),  an  alternative 
expression  for  is  derived,  in  terms  of  time-derivatives  of  the  position  vector, 
namely. 


•  •  • 

r  X  r 

•  •  • 

r  X  r 


(11.23) 


Finally,  e„  can  be  readily  computed  as  the  cross  product  of  the  first  two  vectors  of 
the  Frenet-Serret  triad,  namely. 


(rxr)xr 

^  —  TT^  ..  II  II  .  I 

r  X  r  r 


(11.24) 


The  time-derivatives  of  the  Frenet-Serret  vectors  can  be  computed  by  direct 
differentiation  of  the  expressions  given  above,  namely,  Eqs.  (11.20),  (11.23),  and 
(11.24). 
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11.3  Parametric  Path  Representation 

Only  seldom  is  an  explicit  representation  of  the  position  vector  r  of  a  geometric 
curve  possible  in  terms  of  the  arc  length.  In  most  practical  cases,  alternative 
representations  should  be  used.  The  representation  of  the  position  vector  in  terms 
of  a  parameter  cr,  whatever  its  geometric  interpretation  may  be,  whether  length  or 
angle,  will  henceforth  be  termed  a  parametric  representation  of  the  curve  at  hand. 
The  choice  of  cr  is  problem-dependent,  as  we  illustrate  with  examples. 

Below  we  derive  expressions  for  (a)  the  Frenet-Serret  triad;  (b)  the  curvature 
and  torsion;  and  (c)  the  derivatives  of  the  latter  with  respect  to  the  arc  length.  All 
these  expressions,  moreover,  will  be  given  in  terms  of  derivatives  with  respect  to  the 
working  parameter  cr.  The  key  relation  that  we  will  use  is  based  on  the  chain  rule, 
already  recalled  several  times  earlier.  Thus,  for  any  vector  v(cr), 

dy  dy  da 
ds  da  ds 

However,  the  foregoing  relation  is  not  very  useful  because  we  do  not  have  an  explicit 
representation  of  parameter  cr  in  terms  of  the  arc  length.  Nevertheless,  we  will 
assume  that  these  two  variables,  ^  and  cr,  obey  a  monotonic  relation.  What  this 
means  is  that 


—  >0  (11.25) 

ds 

which  is  normally  the  case.  Under  this  assumption,  moreover,  we  can  write  the 
derivative  of  v  as 


dy  dy  j  da 
ds  dsjda 


where,  apparently. 


ds 

da 


dr 

da 


r'(a)| 


Therefore,  the  derivative  sought  takes  the  form 

dy  v'(cr) 

ds  ||U(cr)|| 

It  goes  without  saying  that  the  same  relation  holds  for  scalars,  i.e., 

dv  v'(a) 


(11.26a) 


ds 


r'(CT)| 


(11.26b) 
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Expressions  for  the  Frenet-Serret  triad  now  follow  immediately,  i.e., 

(11.27a) 
(11.27b) 
(11.27c) 


Now,  paraphrasing  relations  (11.14a  and  b),  we  have 

^  ||r'(g)  xr''(g)|| 

||r'(g)|p 

_  r'(g)  X  r"(g)  •  r'" 

||r'(o')  X  r"(o')p 

the  partial  derivatives  of  the  curvature  and  torsion  with  respect  to  the  arc  length 
being  computed  in  terms  of  the  corresponding  partial  derivatives  with  respect  to  the 
parameter  cr,  which  is  done  with  the  aid  of  the  chain  rule,  i.e.. 


(11.28a) 

(11.28b) 


r'(a) 

r'(g)|| 

r'(g)  X  r"(g) 
r'(o')  X  r"(o')|| 

[r'(o')  X  r"(o')]  X  r'(cr) 
lirCg)  X  r"(g)||||r'(g)|| 


(11.29) 


Expressions  for  /c'(cr)  and  r'(cr),  in  turn,  are  derived  by  a  straightforward  differ¬ 
entiation  of  the  expressions  for  k  and  r  in  terms  of  cr,  as  given  in  Eqs.  (11.28a 
and  b).  To  this  end,  we  first  recall  a  useful  expression  for  the  derivative  of  a  rational 
expression  q(x)  whose  numerator  and  denominator  are  denoted  by  N(x)  and  D(x), 
respectively.  This  expression  is 


1 

D(x) 


[A/^'(x)  —  q(x)DXx)] 


(11.30a) 


Note  that  nothing  prevents  the  numerator  of  the  foregoing  rational  expression  from 
being  a  vector,  and  hence,  a  similar  formula  can  be  applied  to  vector  ratios  as  well. 
Consider  the  vector  rational  function  q(v)  =  n(x)/ D(x).  Hence, 


q'(x) 


1 

D(x) 


[n'(x)  —  q(x)T)'(x)] 


(11.30b) 


As  a  matter  of  fact,  the  above  relation  can  be  extended  to  matrix  numerators.  Not 
only  is  this  possible,  but  the  argument  can  likewise  be  a  vector  or  a  matrix  variable, 
and  similar  formulas  would  apply  correspondingly. 
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We  thus  have,  for  the  derivative  of  the  curvature, 


K'(a)  = 


1 

■  d 

r'(cr)|  ^ 

da 

r'(a)  xr"(a)||  - /c^||r'(a)|| 

da 


(11.31) 


Now  we  find  the  first  term  inside  the  brackets  of  the  foregoing  expression  from  the 
relation 


d 

da 


r'(cr)  X  r"(a)\\^  =  2\\r'  x  r' 


d 

da 


r  X  r 


which  yields 


d 

da 


r'  X  r" 


1  d 


2\\r'  X  r"||  da 


r'(a)  X  r"(a)||' 


But 


^||r'(a)  X  r"(a)|p  =  ^{[r'(CT)  x  r"{a)]  ■  [r'(a)  x  r"(a)]} 
da  da 

=  2[r'(a)  X  r"(a)]  •  x  r"(a)]  (11.32) 

da 


the  derivative  of  the  above  term  in  brackets  reducing  to 

^  [r'(cr)  X  r"(cr)]  =  x  r'"  {a) 


da 


and  hence, 


d  ,,  /  //.,  X  X  Y'"{a)] 

r  X  r 


da 


y'  X  y" 


(11.33a) 


Furthermore, 


d  ,  .  ,  .  d  , 

■r'(a)|p  =  3||r'(a)||Vlk(^)ll 


da 


da 


the  last  derivative  again  being  found  from  an  intermediate  relation,  namely. 


^  "r'(a)f  =  2||r'(a)||-^||r'(a)|| 


da 


da 


whence. 


d 

da 


r'(a)||  = 


1  d 


2||r' (cr) II  da 


r'(a)|( 
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with 


^  ■■r'(a)f  =  -f-[r'(a)  •  r'(a)]  =  2r'(a)  •  r"(a) 


da 


do 


and  so, 


d  ..  ...  r'(o')  •  r"(cr)  d  ^ 


da 


|r'(a)||  = 


|r' (cr)  II  da 


|r'(a)|| 


Therefore, 


d 

da 


|r'(o-)lP  =  3||r'(a)||r'(a)-r"(a) 


(11.33b) 


Substitution  of  Eqs.  (11.33a  and  b)  into  Eq.  (11.31)  yields  the  desired  expression, 
namely. 


,,  ,  [r'(c^)  X  r"(a)]  •  [r'(a)  x  r"'{a)]  r'(a)  •  r”(a) 

K  (cr)  =  - TTTT— - TT - 3/C 


|r'(o')p||r'  X  r" 


|r'(a)||: 


(11.34) 


Likewise, 


r'(a)  =  — 
^  ^  D 


(11.35a) 


with  N  and  D  defined  as 


N  =  -^[r^(o')  X  r"(a)  •  y'"{g)]  —  r-^  ||r' (cr)  x  r"(cr)||^  (11.35b) 

da  da 


D  =  ||r'(cr)  X  r"(cr)|| 


(11.35c) 


The  first  term  of  the  numerator  N  of  the  foregoing  expression  can  be  readily 
calculated  as 

—  [r'(cr)  X  r''(cr)  •  r'''(cr)]  =  r'(cr)  x  y"{g)  •  r^^^^(cr)  (11.35d) 

da 

while  the  derivative  appearing  in  the  second  term  of  the  same  numerator  was 
obtained  previously,  as  displayed  in  Eq.  (11.32).  Upon  substitution  of  the  expres¬ 
sions  appearing  in  Eqs.  (1 1.32)  and  (1 1.35d)  into  Eq.  (1 1.35a),  we  obtain  the  desired 
expression: 


r'(cr)  X  r"(cr)  •  [r^^^^(cr)  —  2rr'(cr)  x  r'"(cr)] 

||r'(cr)  X  r"(cr)p 


(11.35e) 


thereby  completing  the  desired  relations. 
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Table  11.1  DH  parameters 
of  a  Fanuc  S-300  Robot 


Link 

Ui  (m) 

bi  (m) 

ai{° 

1 

0.0 

0.9 

90 

2 

0.9 

0.0 

0 

3 

0.95 

0.0 

90 

4 

0.0 

1.3 

-90 

5 

0.0 

0.0 

90 

6 

0.0 

0.44 

-90 

Example  11.3.1  (Planning  of  a  Gluing  Operation).  A  robot  used  for  a  gluing 
operation  is  required  to  guide  the  glue  nozzle  fixed  to  its  end-effector  through  a 
helicoidal  path  so  that  the  tip  of  the  nozzle  traverses  the  helix  at  a  constant  speed 
Vo  =  0.8  m/s  and  the  end-effector  maintains  a  fixed  orientation  with  respect  to 
the  curve,  i.e.,  with  respect  to  the  Frenet-Serret  triad  of  the  helix.  Determine  the 
orientation  matrix  Q  of  the  end-effector  with  respect  to  a  frame  {v,  y,  z}  fixed 
to  the  robot  base,  as  well  as  the  angular  velocity  and  angular  acceleration  of  the 
end-effector.  The  operation  is  to  be  performed  with  a  Fanuc  S-300  robot,  whose 
Denavit-Hartenberg  (DH)  parameters  are  given  in  Table  11.1,  while  the  axis  of 
the  helix  is  chosen  to  be  parallel  to  the  first  axis  of  the  robot  and  beginning  at 
point  Pq  (2,  —2,  1.2)  in  meters.  Find  the  joint  trajectories  of  the  robot  as  well  as 
the  associated  joint  rates  and  joint  accelerations  from  Cartesian  position,  velocity, 
and  acceleration  data.  Verify  that  the  joint-rate  and  joint- acceleration  profiles  are 
compatible  with  those  of  the  joint  variables.  It  is  known  that  the  radius  of  the  helix 
is  a  =  1.6  m  and  that  its  pitch  is  b  =  2.5  m/tum.  Finally,  the  gluing  operation  takes 
place  in  10  s. 


Solution:  We  will  use  a  Cartesian  frame  fixed  to  the  base  of  the  robot  such  that  its 
z  axis  coincides  with  the  axis  of  the  first  revolute.  The  helix  can  then  be  given  in  the 
parametric  representation  shown  below: 


X  =  2  a  cos  (p 


y  =  —2-hasin(p 


z  —  1.2-1- 


b(p 

27t 


where  the  parameter  cp  is  the  angle  made  by  the  projection,  onto  the  X-Y  plane,  of 
the  position  vector  of  a  point  P  of  the  helix  with  the  v  axis. 

In  the  process,  we  will  need  first  and  second  time-derivatives  of  the  foregoing 
Cartesian  coordinates.  These  are  given  below  for  quick  reference: 


X  =  —a(p  sincp 

y  =  acp  cos  (p 

.  b  . 
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All 


and 


X  =  —a(p  cos  (f  —  a(p  sin  (p 

r\ 

y  =  —a<p  sin  cp  +  acp  cos  (p 
b 

•  •  •• 

z 

ZTT 

We  now  impose  the  constant-speed  condition,  which  leads  to 


=  a^(p^  + 


47r^ 


•  2  2 
(p  =v^ 


and  hence. 


(p  =  c 

where  the  constant  c  is  defined  as 

I  47r^ 

^  y  Ajt^a^  + 

Thus,  (p  is  constant,  and  hence, 

p  =  ct 

Moreover,  in  terms  of  constant  c,  the  Cartesian  coordinates  of  a  point  of  the  helix 
take  on  the  forms 


X  =  2  A-  a  cos  ct 

y  =  —2  A-  a  sin  ct 
be 

z  —  1.2  +  - — t 

271 

the  first  time-derivatives  of  these  coordinates  becoming 


X  =  —ac  sinc^ 


y  =  ac  cos  ct 

be 
z  = 


271 


and  the  corresponding  second  time-derivatives 


X  =  —ac^  cos  ct 
y  =  —ac^  sin  ct 


z  =  0 
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Now  the  Frenet-Serret  triad  is  readily  calculated  as 


Furthermore, 


dr 

ds 


r 

s 


c 

Vo 


—a  sin  ct 
a  cos  ct 
b  jin 


d^t 

ds 


s 


2 

ac 


—  cos  ct 

—  sin  ct 

0 


=  /ce 


n 


from  which  it  is  apparent  that 


c^  


cos  ct 
sin  ct 
0 


Thus,  the  binormal  vector  is  calculated  simply  as  the  cross  product  of  the  first 
two  vectors  of  the  Frenet-Serret  triad,  namely. 


X 


c 

Vo 


—{bjlTt)  sin  ct 
(bjlTt)  cos  ct 
—a 


and  hence,  the  orientation  matrix  Q  of  the  gluing  nozzle,  or  of  the  end-effector  for 
that  matter,  is  given  by 

Q  =  [e,  e„  e*] 


Hence, 


— (2  sinc^  —  (t;o/c) cos (b/27t)sinct 
a  cos  ct  — {vo/ c)  sin  ct  —{b/ In)  cos  ct 
{bjlTt)  0  VL  _ 

Now,  the  angular  velocity  is  determined  from  Eq.  (11.12),  which  requires  the 
calculation  of  the  Darboux  vector,  as  given  in  Eq.  (11.13).  Upon  calculation  of  the 
Darboux  vector  and  substitution  of  the  expression  thus  resulting  into  Eq.  (11.12), 
we  obtain 


0 

"O' 

0 

=  c 

0 

_(47t^a^  +  b^)l47t^_ 

_1_ 

which  is  thus  constant,  and  hence. 


CO  =  0 
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Now,  the  coordinates  of  the  center  of  the  wrist,  C ,  are  determined  with  the  aid 
of  relation  (4.18c),  where  the  operation  point  is  a  point  on  the  helix,  i.e.,  p  =  xi  + 
+  zk,  parameters  and  /X6  being  obtained  from  Table  11.1,  namely. 


be  =  0.440  m,  Xe  =  cosq'6  =  0,  iie  =  sinQ'6  =  —  1 


Furthermore,  the  numerical  value  of  c  is  obtained  from  the  helix  geometry,  namely. 


Upon  substitution  in  Eq.  (4.18c)  of  the  entries  found  above  for  Q,  along  with 
the  numerical  values,  we  obtain  the  Cartesian  coordinates  of  the  center  C  of  the 
spherical  wrist  of  the  robot  as 


2  +  1.16cos(0.48522^) 
-2+  1.16  sin  (0.485220 
1.2  +  0.19306^ 


yc 

_^c_ 


in  meters.  Apparently,  point  C  describes  a  helicoidal  path  as  well,  although  of  a 
smaller  radius,  that  is  coaxial  with  the  given  helix. 

Now  the  time-histories  of  the  joint  angles  are  computed  from  inverse  kinematics. 
Note  that  the  robot  at  hand  being  of  the  decoupled  type,  it  allows  for  a  simple 
inverse  kinematics  solution.  The  details  of  the  solution  were  discussed  extensively 
in  Sect.  4.4  and  are  left  as  an  exercise  to  the  reader. 

Of  the  four  inverse  kinematics  solutions  of  the  arm,  three  were  found  to  lead 
to  link  interferences,  when  these  trajectories  were  tested  with  the  aid  of  RVS,  the 
package  for  robot  visualization  developed  at  McGill  University  (Darcovich  et  al. 
1999).  Hence,  only  one  such  solution  is  physically  possible.  This  solution,  along 
with  one  of  the  two  wrist  solutions,  is  plotted  in  Fig.  11.1,  with  Figs.  11.2  and  11.3 
showing,  respectively,  the  corresponding  joint  rates  and  joint  accelerations. 

Note  that  the  maxima  and  minima  of  the  joint- variables  occur  at  instants  where 
the  corresponding  joint  rates  vanish.  Likewise,  the  maxima  and  minima  of  joint  rates 
occur  at  instants  where  the  associated  joint  accelerations  vanish,  thereby  verifying 
that  the  computed  results  are  compatible.  A  more  detailed  verification  can  be  done 
by  numerical  differentiation  of  the  joint- variable  time-histories. 

Example  11.3.2  (Planning  of  an  Arc-Welding  Operation).  A  spherical  reservoir  of 
radius  R  is  to  be  arc- welded  to  a  cylindrical  pipe  of  radius  r,  with  the  axis  of  the 
cylinder  located  a  distance  d  from  the  center  of  the  sphere,  all  elements  of  the 
cylinder  piercing  the  sphere,  i.e.,  J  +  r  <  R,  as  shown  in  Fig.  11.4.  Note  that  two 
intersection  curves  are  geometrically  possible,  but  the  welding  will  take  place  only 
along  the  upper  curve.  Moreover,  the  welding  electrode  is  to  traverse  the  intersection 
curve,  while  the  tool  carrying  the  electrode  is  to  keep  a  constant  orientation  with 
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t  (s) 


t  (s) 


Fig.  11.1  Joint  trajectories  for  a  Fanuc  S-300 


respect  to  that  curve.  In  the  coordinate  frame  shown  in  Fig.  11.4,  find  an  expression 
for  the  rotation  matrix  defining  the  orientation  of  the  end-effector,  to  which  the 
electrode  is  rigidly  attached. 

Solution:  Note  that  the  X  axis  of  the  coordinate  frame  indicated  in  Fig.  11.4 
intersects  the  A  axis  of  the  cylinder,  this  axis  being  parallel  to  the  Z  axis.  Moreover, 
we  define  cp  as  the  angle  shown  in  Fig  1 1 .4b.  Now,  the  v  and  y  coordinates  of  an 
arbitrary  point  of  the  intersection  curve  are  given  by 

X  =  d  -j-  r  cos  cp 


y  =  r  ^mcp 


(11.36a) 

(11.36b) 


11.3  Parametric  Path  Representation 
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Fig.  11.2  Joint  velocities  for  a  Fanuc  S-300 


Further,  in  order  to  find  the  remaining  z  coordinate,  we  use  the  equation  of  the 
sphere,  S,  namely, 

5:  x^  +  y^  +  z^  = 

If  we  substitute  the  x  and  y  coordinates  of  the  intersection  curve  in  the  above 
equation  and  then  solve  for  the  z  coordinate  in  terms  of  (^,  we  obtain 

z  =  ±y/R^-r^  —  d^—2drcos(p  (11.36c) 

In  the  above  relation,  the  plus  and  minus  signs  correspond  to  the  upper  and  lower 
portions  of  the  intersection  curve,  respectively.  Since  we  are  interested  in  only  the 
upper  intersection,  we  will  take  only  the  positive  sign  in  that  relation.  Furthermore, 
we  define 
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^3 

(deg/s^) 


t  (s) 


0 


6 


t  (s) 


Fig.  11.3  Joint  accelerations  for  a  Fanuc  S-300 


d  =  Xr,  R  =  iir 


where  X  and  /x  are  nondimensional  constants.  Moreover,  let 


=  /x^  —  —  1  >  0 


(f  = 


—  2X  cos  (p 


the  inequality  following  from  the  geometry  of  Fig.  11.4b.  Then,  the  position  vector 
r  of  any  point  on  the  intersection  curve  can  be  expressed  in  the  form 


X  +  cos  cp 
map 

.  1/^  . 


Y  =  r 


(11.37) 


11.3  Parametric  Path  Representation 
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Fig.  11.4  Intersection  curve  between  a  spherical  reservoir  and  a  cylindrical  pipes 


Now,  upon  differentiation  of  r  with  respect  to  (^,  we  obtain 


r'(^)  =  r 


y"{(p)  =  r 


where  we  have  used  the  relation 


—  sin^ 
cos  cp 
sin^_ 


—  cos  (p 

—  sin^ 

_X(p  cos  (p  —  (A^  sin^ 


(11.38a) 


(11.38b) 


0  (SP)  =  mi(p)(p 

In  addition,  using  Eqs.  (11.38a  and  b),  we  derive  the  items  needed  to  compute  the 
Frenet-Serret  triad,  from  which  we  will  derive  the  required  orientation  matrix,  i.e.. 


r'(^)  X  y"{(p)  = 


X(p  —  X^cp^  cos  (p  sin^  cp 
—X^cp^  sin^  (p 

1 


||r'(^)||  =  rG{(p) 

\y'{(p)  X  v"{(p)\\  =  XiiX^Diip) 


(11.39a) 

(11.39b) 

(11.39c) 


with  functions  D{(p)  and  G(^)  defined  as 

D  =  p^X^  +  —  6p^A(A^  +  p^)  cos  cp  +  6A^(A^  +  2p^)  cos^  cp 

+  2A^(p^  —  4)  cos^  (p  —  3A^  cos^  cp  (11.39d) 


G  = 


1  +  X'^cp^  sin^  cp 


(11.39e) 
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Now  ,  and  e„  are  obtained  as 


r'(^) 

r'{(p)\\ 


1 

G 


—  sin^ 
cos  (p 
sin^ 


G 


(11.40a) 


r'(^)  X  r"((p) 
r'(^)  xr"(^)|| 


1 


X(p  —  X^(p^  cos  (p  sin^  (p 
—X^cp^  sin^  cp 

1 


0^^ 


(11.40b) 


1 

0^^/DG 


—X^0^  sin"^  (p  —  cos  (p 
X^0^  cos  cp  sin^  (p  —  X^0^  sin  cp  —  sin  cp 
X0  cos  (p  —  X^0^  sin^  (p 


0^Vdg 


(11.40c) 


where  e„  has  been  calculated  as  e„  =  x  . 

The  orthogonal  matrix  defining  the  orientation  of  the  end-effector  can  now  be 
readily  computed  as 


Q  =  [e,  e„  e^] 

for  we  have  all  the  necessary  expressions.  Note,  however,  that  these  expressions 
allow  us  to  find  Q  for  any  value  of  cp,  but  we  do  not  have,  as  yet,  an  expression  of 
the  form  (p{t)  that  would  allow  us  to  obtain  Q(0-  Such  an  expression  is  derived  in 
Example  11.5.1. 

Example  11.3.3  ( Calculation  of  Torsion,  Curvature,  and  Darboux  Vector).  We  refer 
here  to  the  intersection  curve  of  Example  11.3.2,  for  which  we  want  to  find 
expressions  for  its  curvature,  torsion,  and  Darboux  vector. 

Solution:  We  can  use  directly  the  expressions  derived  above,  Eqs.  (1 1.28a  and  b), 
to  obtain  the  curvature  and  torsion  in  terms  of  derivatives  with  respect  to  parameter 
(p.  With  these  expressions  and  those  for  the  Erenet-Serret  triad,  the  Darboux  vector 
would  follow.  However,  we  can  take  shortcuts,  for  we  already  have  expressions  for 
the  Erenet-Serret  triad,  if  we  express  the  curvature  and  torsion  in  terms  of  this  triad 
and  its  derivatives  with  respect  to  as  we  explain  below.  Indeed,  from  the  Erenet- 
Serret  relations,  Eqs.  (11.2b),  we  can  express  the  curvature  and  torsion  in  the  forms 


/c  =  ej(^)-e„  (11.41a) 

r  =  -e'(^).e„  (11.41b) 


and  hence,  all  we  need  now  are  the  derivatives  of  the  tangent  and  normal  vectors 
with  respect  to  These  are  readily  derived  using  relation  (1 1.26a),  i.e.. 


11.3  Parametric  Path  Representation 
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e'fis)  = 

^tiv) 

(11.42a) 

^b(^)  = 

<(<P) 

l|r'('P)ll 

(11.42b) 

Now,  in  order  to  differentiate  the  Frenet-Serret  triad  with  respect  to  cp,  wq  first 
note,  from  Eqs.  (11 .40a-c),  that  these  three  expressions  are  vector  rational  functions, 
and  hence,  their  derivatives  with  respect  to  cp  are  derived  by  applying  Eq.  (11.30b), 
thereby  obtaining 

2^\\ 

where  and  are  the  numerators  of  the  vector  rational  expressions  of  and 
e^,  respectively,  given  in  Eq.  (11.40a  and  b).  Below  we  calculate  the  foregoing 
derivatives  with  respect  to  cp: 


= 


(p- 


Vd 


((p)  -  et 


?>(fP'(p\(p)-/D  +  0 


(11.43) 

(11.44) 


—  cos  (p 

—  sin^ 

A(^(cos  (p  —  X0^  sin^  cp) 


(^)  =  ^ 


0'  —  X0^  sm(p[30'  cos  (p  sm(p  0(3  cos^  (p 
—3X0^  sin^  (p[0'  ^mcp  -\-  0  cos  cp] 

0 


—X  sin  (p 

(p^  —  2X  cos  cpyi^ 


D  '((p)  =  6p^X(X^  +  p^)  sin  cp  —  \2X^(X^  +  2p^)  cos  (p  sin  cp 
—  6X  (p  —  4)  cos  (psincp  12X  cos  cp  sin  cp 


G'(cp)  = 


}?  sin  (p 
2G 


(20  cos  (p  -\-  0'  sin  (p) 


and  ||r'(^)||  was  already  calculated  in  Example  11.3.2. 
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If  we  now  substitute  all  the  foregoing  expressions  into  Eqs.  (11.42a  and  b),  we 
obtain,  after  intensive  simplifications, 


K 


r 


GV 

^  sin  (p 
rDG^ 


with  function  E  {(p)  defined,  in  turn,  as 


(11.45a) 

(11.45b) 


E{(p)  =  sin^  (p  +  Xcp^  sin^  (p{X  cos  ^  —  1)  +  cos  cp]  (11.46) 

T 

With  the  foregoing  expressions  for  e^,  e^,  r,  and  /c,  computing  the  Darboux  vector  of 
the  intersection  curve  reduces  to  a  routine  substitution  of  the  foregoing  expressions 
into  Eq.  (11.13). 


11.4  Parametric  Splines  in  Trajectory  Planning 

Sometimes  the  path  to  be  followed  by  the  tip  of  the  end-effector  is  given  only  as  a 
discrete  set  of  sampled  points  {Pi  This  is  the  case,  for  example,  if  the  path  is  the 
intersection  of  two  warped  surfaces,  as  in  the  arc- welding  of  two  plates  of  the  hull 
of  a  vessel  or  the  spot- welding  of  two  sheets  of  the  fuselage  of  an  airplane.  In  these 
instances,  the  coordinates  of  the  sampled  points  are  either  calculated  numerically 
via  nonlinear-equation  solving  or  estimated  using  a  vision  system.  In  either  case,  it 
is  clear  that  only  point  coordinates  are  available,  while  trajectory  planning  calls 
for  information  on  derivatives  of  the  position  vector  of  points  along  the  path 
with  respect  to  the  arc  length.  These  derivatives  can  be  estimated  via  a  suitable 
interpolation  of  the  given  coordinates.  Various  interpolation  schemes  are  available 
(Eoley  and  Van  Dam  1982;  Hoschek  and  Lasser  1992),  the  most  widely  accepted 
ones  being  based  on  spline  functions,  which  were  introduced  in  Sect.  6.6.  The 
splines  introduced  therein  are  applicable  whenever  sl  function,  not  a  geometric  curve, 
is  to  be  interpolated.  However,  in  trajectory  planning,  geometric  curves  in  three- 
dimensional  space  come  into  play,  and  hence,  those  splines,  termed  nonparametric, 
are  no  longer  applicable.  What  we  need  here  are  parametric  splines,  as  described 
below. 

Although  parametric  splines,  in  turn,  can  be  of  various  types  (Dierckx  1993),  we 
will  focus  here  on  cubic  parametric  splines  because  of  their  simplicity. 

Let  Pi  (xi ,  yi ,  Zi),  for  i  =  1, . . . ,  V,  be  the  set  of  sampled  points  on  the  path 
to  be  traced  by  the  tip  of  the  end-effector,  {p/  being  the  set  of  corresponding 
position  vectors.  Our  purpose  in  this  section  is  to  produce  a  smooth  curve  E  that 
passes  through  (Pi  and  that  has  a  continuous  Erenet-Serret  triad.  To  this  end,  we 
will  resort  to  the  expressions  derived  in  Sect.  1 1.3,  in  terms  of  a  parameter  a,  which 
we  will  define  presently. 


1 1.4  Parametric  Splines  in  Trajectory  Planning 
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We  first  introduce  a  few  definitions:  Let  the  kih  derivative  of  the  position  vector 
p  of  an  arbitrary  point  P  oi  F  with  respect  to  cr,  evaluated  at  Pi,  be  denoted  by 
p ,  its  components  being  denoted  correspondingly  by  xf^ ,  ,  and  zf^ .  Next,  the 

coordinates  of  P  are  expressed  as  piecewise  cubic  polynomials  of  cr,  namely. 


x(a)  =  A^i{o  -  Oi  f  +  Bxiia  -  a,)^  +  C^i{a  -  a,)  +  D^i  (11.47a) 
y(a)  =  Ayi (a  -  ct,)^  +  Byi (a  -  Uif  +  Cyt (a  -  ct,)  +  Dyi  (1 1.47b) 

-  (Ji  f  +  C^i{o  -  Gi)  +  D^i  (11.47c) 

for  a  real  parameter  cr,  such  that  cr/  <  cr  <  cr/+i,  and  i  =  ,  N  —  with  cr/ 

defined  as 

Gi  =  0,  CT,+i  =  Gi  +  Act,-,  Act,-  =  ^ Axf  +  Ayf  +  Azj  (11.47d) 
Ax,-  =  x,-+i  -  X,-,  Ayi  =  yi+i  -  yi ,  Az,-  =  Zi+i  -  n  (11.47e) 


and  hence,  Acr/  represents  the  length  of  the  chord  subtended  by  the  arc  of  path 
between  Pf  and  Pt+i.  Likewise,  cr  denotes  a  path  length  measured  along  the  spatial 
polygonal  joining  the  N  points  {  L’/jf .  Thus,  the  closer  the  aforementioned  points, 
the  closer  the  approximation  of  Acr/  to  the  arc  length  between  these  two  points,  and 
hence,  the  better  the  approximations  of  the  curve  properties. 

The  foregoing  spline  coefficients  A^i ,  Ayi ,  . . . ,  D^i ,  for  /  =  1, . . . ,  A  —  1,  are 
determined  as  explained  below.  Let  us  define  the  A -dimensional  vectors 

^  ^  mm  mm  mm  ^  M  ^ 

X  =  [xi, . . . ,  xn]  ,  X  =[X|,...,x^]  (lL48a) 

y  =  [yi, . . .  y”  =  [yi,  ■■■,)>  nV  (11.48b) 

z=  [zi,...,ZNf,  z"  =  [z'I,...,Zn]^  (11.48c) 

The  relationships  between  x,  y,  and  z  and  their  counterparts  x",  y",  and  z"  are  the 
same  as  those  found  for  nonparametric  splines  in  Eq.  (6.58a),  namely. 


Ax"  =  6Cx  (11. 49a) 

Ay"  =  6Cy  (11. 49b) 

Az"  =  6Cz  (11. 49c) 

which  are  expressions  similar  to  those  of  Eq.  (6.58a),  except  that  the  A  and  C 
matrices  appearing  in  Eq.  (11. 49b)  are  now  themselves  functions  of  the  coordinates 
of  the  supporting  points  (SP)  of  the  spline.  In  fact,  the  (N  —2)  x  N  matrices  A  and 

C  are  defined  exactly  as  in  Eqs.  (6.58b  and  c),  repeated  below  for  quick  reference: 
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ai  2q'i,2  Oil  0 
0  a2  2^2,3  0i2 


0  O' 

0  0 


0  0 
0  0 


. . .  oijsi'"  2oi]\j'  /'  oijsi"  0 

0  •••  oi]^"  oif\f'_ 


and 


—^1,2  Pi  0 
0  P2  —Pi, 3  Pi 


0  O' 

0  0 


0  0  •••  Pn'"  — Pn',"n"  Pn"  0 
0  0  0  •••  Pn"  — Pn,"n'  Pn' 


(11.49d) 


(11.49e) 


where  ak  and  Pk  are  now  defined  correspondingly,  i.e.,  fori,  j,k  =  1, . . . ,  N' , 


at  =  Adk,  atj  =  at  +  aj,  pk  =  l/oik,  Pi,j  =  Pi  +  Pj  (11.50) 
while  N',  N",  and  N'"  are  defined  as  in  Eq.  (6.58f),  i.e.,  as 

N'  =  N-l,  N"  =  N-2,  N'"  =  N-3  (11.51) 


Note  that  the  spline  p(cr)  is  fully  determined  once  its  coefficients  are  known. 
These  are  computed  exactly  as  their  counterparts  for  nonparametric  splines,  namely, 
as  in  Eqs.  (6.55a-e).  Obviously,  different  from  the  aforementioned  formulas,  the 
coefficients  of  the  parametric  spline  pertain  to  three  coordinates,  and  hence,  three 
sets  of  such  coefficients  need  be  computed  in  this  case.  In  order  to  simplify  matters, 
we  introduce  the  vectors  below: 


^xk 

Bxk 

[Cxkl 

Dxk 

= 

1 

_ 1 

,  bfc  = 

Byk 

.Bzk_ 

1  — 

1 

_ 1 

,  dk  = 

Dyk 

.D,k_ 

(11.52) 

and  thus,  the  position  vector  of  an  arbitrary  point  P  on  the  parametric  spline  takes 
on  the  form 


p(a)  =  a*:(a-CT*:)^+b*:(a-a*:)^  +  Ci;(a-CTi;)+di;,  k  =  (11.53a) 
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in  the  interval  <  cr  <  cr^+i.  The  counterpart  set  of  Eqs.  (6.55a-e)  is  then 


=  ^^(P^'+i-pD  (ll-53b) 

b,  =  lp"  (11.53c) 

i  Aafc  (piVi  +  2p;0  (1 1.53d) 

4  =  Pit  (11.53e) 

Ap,t  =  P/t+i  -  P/t  (11.531) 

where  vectors  p^  and  p^'  are  defined  as 

~Xk~\  rxf 

P^=  Jk  ,  (11-54) 

-^k  A  L  _ 


Note  that  since  p  is  piecewise  cubic  in  cr,  p'  is  piecewise  quadratic,  whereas  p" 
is  piecewise  linear  in  the  same  argument,  p'"  being  piecewise  constant;  higher- 
order  derivatives  vanish.  Properly  speaking,  however,  the  piecewise  constancy  of  p'" 
causes  the  fourth-order  derivative  to  be  discontinuous  at  the  SP,  and  consequently, 
all  higher-order  derivatives  are  equally  discontinuous  at  those  points.  In  practice, 
these  discontinuities  are  smoothed  out  by  the  inertia  of  the  links  and  the  motors,  if 
the  SP  are  chosen  close  enough.  Obviously,  higher-order  continuity  can  be  achieved 
if  higher-order  splines,  e.g.,  quintic  splines,  are  used  instead.  For  the  sake  of 
conciseness,  these  splines  are  not  discussed  here,  the  interested  reader  being  directed 
to  the  specialized  literature  (Dierckx  1993). 

Further,  the  N  x3  matrices  P  and  P"  are  defined  as 


r  T~\ 

Pi 

p  = 

T 

P2 

,  p"  = 

(pD^ 

(11.55) 

1 

' 

1 _ 

which  allows  us  to  rewrite  Eqs.  (1 1.49b)  in  matrix  form  as 

AP''  =  6CP  (11.56) 

It  is  now  apparent  that  the  spline  coefficients  ,  . . . ,  can  be  calculated  once  vec¬ 
tors  p^'  are  available.  These  vectors  can  be  computed  via  matrix  P''  as  the  solution 
to  Eq.  (11.56).  However,  finding  this  solution  requires  inverting  the  {N  —2)  x  N 
matrix  A,  which  is  rectangular  and  hence  cannot  be  inverted,  properly  speaking. 
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We  thus  have  an  underdetermined  system  of  linear  equations,  and  further  conditions 
are  needed  in  order  to  render  it  determined.  Such  conditions  are  those  defining  the 
type  of  spline  at  hand.  For  example,  closed  paths  call  naturally  for  periodic  splines, 
while  open  paths  call  for  other  types  such  as  natural  splines.  The  conditions  imposed 
on  periodic  parametric  splines  are  listed  below: 

Pn  =  Pi,  p'at  =  pi,  Pn  =  Pi  (11.57a) 

On  the  other  hand,  natural  parametric  splines  are  obtained  under  the  conditions 

Pi  =0  (11.57b) 

Thus,  if  a  periodic  parametric  spline  is  required,  then  vectors  p^y  and  p'^  can  be 
deleted  from  matrices  P  and  P'',  respectively,  these  then  becoming  (A^  —  1)  x  3 
matrices,  namely. 


r  r  n 

Pi 

r  (Pi)^  1 

p  = 

T 

Pi 

,  P"  = 

(11.58) 

1 

L _ 

1 

1 _ 

Moreover,  the  first-derivative  condition  of  Eq.  (11.57a)  is  added  to  the  N  —  2 
continuity  conditions  of  Eq.  (6.56),  thereby  obtaining  N  —  I  equations  of  this  form. 
Consequently,  A  becomes  an  (A  —  1)  x  (A  —  1)  matrix.  Correspondingly,  C  also 
becomes  an  (A  —  1)  x  (A  —  1)  matrix,  i.e.. 


and 


ai 

0 

0 

OiN' 


ai  0  0 

20' 1,2  0'2  0 
a2  20^2, 3  0i3 


0  ...  otfif'"  2of^wyv" 

0  0  •••  a  N't 


OiN' 

0 

0 


01 N" 

2aN",N' 


— ^1  0  0 

Pi  ~Pl,2  Pi  0 

0  P2  —Pl,!  Pi 


Pn' 

0 

0 


0  •••  Pn'"  — Pn'",n"  Pn" 

0  0  •••  Pn"  — Pn",n' 


(11.59a) 


0 

Pn' 


(11.59b) 
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Table  11.2  The  Cartesian  coordinates  of  the  supporting  points 


p 

0° 

o 

o 

o 

O 

o 

o 

120° 

150° 

X 

0.45 

0.429904 

0.375 

0.3 

0.225 

0.170096 

y 

0 

0.075 

0.129904 

0.15 

0.129904 

0.075 

z 

0.396863 

0.411774 

0.45 

0.497494 

0.540833 

0.570475 

p 

oo 

o 

o 

210° 

240° 

270° 

300° 

330° 

X 

0.15 

0.170096 

0.225 

0.3 

0.375 

0.429904 

y 

0 

-0.075 

-0.129904 

-0.15 

-0.129904 

-0.075 

z 

0.580948 

0.570475 

0.540833 

0.497494 

0.45 

0.411774 

Since  A  is  nonsingular,  Eq.  (1 1.56)  can  be  solved  for  P",  namely, 

P"  =  6A“^CP  (11.60) 

thereby  computing  all  vectors  from  which  can  be  readily  obtained. 

Hence,  the  spline  coefficients  follow. 

Likewise,  if  natural  parametric  splines  are  used,  then  P"  becomes  an  (A  —  2)  x  3 
matrix,  while  A,  consequently,  becomes  an  (N  —  2)  x  (N  —  2)  matrix,  as  given  in 
Eq.  (6.59). 

Example  11.4.1  (Spline -Approximation  of  a  Warped  Path).  Eor  the  numerical  val¬ 
ues  R  =  0.6  m,  r  =  0.15  m,  and  d  =  0.3  m,  determine  the  periodic  parametric 
cubic  spline  approximating  the  intersection  of  the  sphere  and  the  cylinder  of 
Eig.  11.4,  with  12  equally  spaced  supporting  points  along  the  cylindrical  coordinate 
ip,  i.e.,  with  supporting  points  distributed  along  the  intersection  curve  at  intervals 
ISip  =  30°.  Using  the  spline,  find  values  of  the  tangent,  normal,  and  binormal 
vectors  of  the  curve,  as  well  as  the  rotation  matrix  Q.  In  order  to  quantify  the  error 
in  this  approximation,  compare  (a)  the  components  of  the  two  position  vectors,  the 
exact  and  the  spline-generated  ones,  while  normalizing  their  differences  using  the 
radius  of  the  cylinder  r;  and  (b)  the  Euler-Rodrigues  parameters  of  the  exact  and 
the  spline-approximated  rotation  matrices.  Plot  these  errors  vs.  (p. 

Solution:  We  use  Eq.  (11.37)  to  find  the  Cartesian  coordinates  of  the  supporting 
points.  The  numerical  results  are  given  in  terms  of  the  components  of  r  = 
[x,  y,  zY  ill  Table  11.2.  Note  that  this  table  does  not  include  the  Cartesian- 
coordinate  values  at  360°  because  these  are  identical  with  those  at  0°. 

The  four  Euler-Rodrigues  parameters  {ri}]^^  of  the  rotation  matrix  are  most 
suitably  calculated  in  terms  of  the  linear  invariants,  i.e.,  as  appearing  in  Eq.  (2.79). 
If  we  let  p  and  f  denote  the  estimates  of  p  and  r,  respectively,  then  the  orientation 
error  is  evaluated  via  the  four  differences  Ar^  =  —  f/,  for  /  =  0, . . . ,  3.  The 

positioning  error  is  computed,  in  turn,  as  the  normalized  difference  e  =  (p-p)/  r  to 
yield  a  dimensionless  number,  its  components  being  denoted  by  and  The 

components  of  the  two  errors  are  plotted  vs.  (p  in  Eigs.  11.5  and  11.6.  Note  that  the 
orientation  errors  are,  roughly,  one  order  of  magnitude  greater  than  the  positioning 


errors. 
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^  (deg) 

-3 


^  (deg) 

Fig.  11.5  Plots  of  the  positioning  errors 


11.5  Continuous-Path  Tracking 

When  a  continuous  trajectory  is  to  be  tracked  with  a  robot,  the  joint  angles  have 
to  be  calculated  along  a  continuous  set  of  poses  of  the  end-effector.  In  practice,  the 
continuous  trajectory  is  sampled  at  a  discrete  set  of  close-enough  poses  {  along 
the  continuous  trajectory.  Then  in  principle,  an  IKP  must  be  solved  at  each  sampled 
pose.  If  the  manipulator  is  of  the  decoupled  type,  these  calculations  are  feasible  in 
a  fraction  of  a  millisecond,  for  the  solution  reduces,  in  the  majority  of  the  cases, 
to  a  cascading  of  quadratic  equations.  In  the  worst  case,  the  inverse  kinematics  of 
a  decoupled  manipulator  requires  finding  all  the  roots  of  a  quartic  equation  at  each 
sampled  pose,  but  this  is  still  feasible  in  the  same  time  frame,  for  the  four  roots 
of  interest  can  be  calculated  from  formulas.  However,  if  the  manipulator  has  an 
architecture  not  lending  itself  to  a  simple  solution  and  requires  solving  polynomials 
of  a  degree  higher  than  four,  then  finding  all  solutions  at  each  sample  pose  may 
require  a  few  milliseconds,  which  may  be  too  slow  in  fast  operations.  Hence,  an 
alternative  approach  is  needed. 

The  alternative  is  to  solve  the  IKP  iteratively.  That  is,  if  we  have  the  value  of 
the  vector  of  joint  variables  0{tk)  and  want  to  find  its  value  at  4+i,  then  we  use 
Algorithm  11.5.1. 
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Fig.  11.6  Plots  of  the  orientation  errors 


Algorithm  11.5.1 

9  ^  9  (4) 

1  find  correction  AO 

if  II  AO  II  <  e,  then  stop; 
else 

0^0  -\-  AO 
go  to  1 


Various  procedures  are  available  to  find  the  correction  of  Algorithm  11.5. 
The  one  we  have  found  very  convenient  is  based  on  the  Newton-Gauss  method 
(Dahlquist  and  Bjorck  1974).  In  the  realm  of  Newton  methods — there  are  several 
of  these,  the  Newton-Gauss  and  the  Newton-Raphson  methods  being  two  of  this 
class — the  closure  equations  (4.9a  and  b)  are  written  in  the  form 


f(6l)  =  Sd 


(11.61) 
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where  is  the  seven-dimensional  prescribed-pose  array.  We  recall  here  the 
definition  of  the  pose  array  introduced  in  Sect.  3.2  to  represent  s^,  namely, 


= 


(11.62) 


with  q  and  defined,  in  turn,  as  a  three-dimensional  vector  invariant  of  the 
rotation  Q  and  its  corresponding  scalar,  respectively.  Moreover,  p  is  the  position 
vector  of  the  operation  point.  Therefore,  the  seven-dimensional  vector  f  is  defined, 
correspondingly,  as 


'uoy 

q 

f(^)  = 

W) 

= 

q^ 

jpm. 

_p_ 

(11.63) 


where  f„(^)  denotes  the  counterpart  of  q  above,  as  pertaining  to  the  product 
Qi  Qe  of  Eq.  (4.9a);  /o(^)  is  the  counterpart  of  as  pertaining  to  the  same 
product;  and  fp(0)  is  the  sum  ai  +  •••  +  Qi---Q5a6.  In  principle,  any  of 
the  three  types  of  rotation  invariants  introduced  in  Sect.  3.2  can  be  used  in  the 
above  formulation. 

Now,  Eq.  (11.61)  represents  a  nonlinear  system  of  seven  equations  in  six 
unknowns.  The  system  is  thus  overdetermined,  but  since  the  four  rotational  equa¬ 
tions  are  consistent,  this  system  should  admit  an  exact  solution,  even  if  this  solution 
is  complex.  Eor  example,  if  p  is  specified  in  sj  above  as  lying  outside  of  the 
manipulator  reach,  then  no  real  solution  is  possible,  and  the  solution  reported  by 
any  iterative  procedure  capable  of  handling  complex  solutions  will  be  complex. 

Upon  application  of  the  Newton-Gauss  method  to  find  a  solution  of  Eq.  (1 1.61), 
we  assume  that  we  have  an  initial  guess  0^,  and  based  on  this  value,  we  generate  a 
sequence  0^ ,  . . 0\  until  either  a  convergence  or  an  abortion  criterion  is 

met.  This  sequence  is  generated  in  the  form 


0i+i  ^  0i 


with  calculated  from 


(11.64) 


=  -f((90  +  sj 


(11.65) 


and  O  defined  as  the  Jacobian  matrix  of  f(^  )  with  respect  to  0 .  Note  that  by  virtue  of 
its  definition,  O  is  a  7  x  6  matrix.  A  common  misconception  in  the  robotics  literature 
is  to  confuse  this  Jacobian  matrix  with  the  Jacobian  defined  by  Whitney  (1972)  and 
introduced  in  Eq.  (5.10a),  which  maps  joint  rates  into  the  EE  twist.  The  difference 
between  the  two  Jacobians  being  essential,  it  is  made  clear  in  the  discussion  below. 
Eirst  and  foremost,  O  is  an  actual  Jacobian  matrix,  while  Whitney’s  Jacobian, 
properly  speaking,  is  not.  In  fact,  O  is  defined  as 

3f 


O  = 


do 


(11.66) 
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In  order  to  find  O  in  Eq.  (1 1.65),  we  note  that  by  application  of  the  chain  rule, 


(11.67) 


However,  from  the  definition  of  f,  we  have  that  f  is  the  time-derivative  of  the  pose 
array  of  the  EE,  i.e.,  s.  Moreover,  by  virtue  of  Eq.  (3.78),  this  time-derivative  can  be 
expressed  as  a  linear  transformation  of  the  twist  t  of  the  EE,  i.e., 

f=Tt  (11.68a) 


with  T  defined  in  Sect.  3.2  as 


F  O43 

O33  I33 


(11.68b) 


where  O33  and  O43  denote  the  3x3  and  the  4x3  zero  matrices,  I33  being  the  3x3 
identity  matrix.  Eurther,  matrix  F  takes  on  various  forms,  depending  on  the  type  of 
rotation  representation  adopted,  as  discussed  in  Sect.  3.2. 

We  write  next  the  left-hand  side  of  Eq.  (1 1.68a)  as  shown  in  Eq.  (1 1.67),  and  the 
twist  t  of  the  right-hand  side  of  Eq.  (1 1.68a)  in  terms  of  ^ ,  as  expressed  in  Eq.  (5.9), 
thereby  obtaining 


^0=TJ0  (11.69) 

which  is  a  relation  valid  for  any  value  of  ^ .  As  a  consequence,  then, 

0=TJ  (11.70) 

whence  the  relation  between  the  two  Jacobians  is  apparent.  Note  that  Eq.  (11.68a) 
allows  us  to  write 


f=TJ^  (11.71) 

Upon  equating  the  right-hand  sides  of  Eqs.  (11.71)  and  (11.68a),  we  obtain 

TJ^=Tt  =  s^  (11.72) 

If  linear  invariants  are  used  to  represent  the  rotation,  then  T  becomes  rank- 
deficient  if  and  only  if  the  angle  of  the  rotation  becomes  n  (Tandirci  et  al.  1994); 
otherwise,  T  is  always  of  full  rank,  and  Eq.  (11.72)  leads  to 


30  =  t 


(11.73) 
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which  is  exactly  the  same  as  Eq.  (5.9).  Now  we  multiply  both  sides  of  the  foregoing 
equation  by  Ac  thereby  obtaining 


JA0=tAt  (11.74) 

All  we  need  now  is,  apparently,  the  product  in  the  right-hand  side  of  the  above 
equation,  namely. 


tA^ 


(oAt 

(oAt 

_pAt 

.  _ 

(11.75) 


The  product  coAt  is  found  below,  in  terms  of  the  orientation  data  available:  First 
and  foremost,  it  is  common  practice  in  the  realm  of  Newton  methods  to  assume 
that  a  good  enough  approximation  to  the  root  sought  is  available,  and  hence,  A^ 
is  “small.”  That  is,  we  assume  that  ||  A^  ||  is  small,  where  ||  •  ||  denotes  any  vector 
norm.  Moreover,  we  use  the  end-effector  pose  at  ^  =  4  as  a  reference  to  describe 
the  desired  pose  at  ^  =  4+i,  the  rotation  sought — that  takes  the  EE  to  its  desired 
attitude — being  denoted  by  AQ,  and  defined  as  (AQ)Qk  =  Qd,  when  all  rotations 
are  expressed  in  the  same  frame  and  Qk  represents  the  orientation  of  the  EE  at 
t  =  tk.  Thus, 

AQ  =  QdQk'^  (11.76) 

Now  we  relate  (oAt  with  AQ.  To  this  end,  notice  that 


(oAt=  vect(S^  A^) 


(11.77a) 


with  ^  denoting  the  cross-product  matrix  of  o).  On  the  other  hand,  AQ  is  bound  to 
be  a  rotation  about  an  axis  parallel  to  a  unit  vector  e,  through  a  small  angle  A0,  and 
hence,  from  Eq.  (2.49), 


AQ  1  +  (A0)E  (11.77b) 

where  E  is  the  cross-product  matrix  of  e.  It  is  then  possible  to  assume  that  Ac 
as  appearing  in  Eq.  (11.77a),  is  the  skew- symmetric  component  of  AQ,  as  given  by 
Eq.  (11.77b),  i.e.. 


AQ  =  l-\-^At 


whence 


QAt=QdQl-l 


which  readily  leads  to 

(oAt  =  vect(QjQ^)  (11.78) 

thereby  obtaining  the  relation  sought. 
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Algorithm  11.5.2 


1  AQ  ^ 


Ap 

At 

A(9 


Pj  -Vk 

vect(AQ) 
_  Ap 

J-^At 


if  II  A0  II  <  6,  then  stop; 
else 


0  ^  0 -\- AO 

Qk  ^  (AQ)Qk 

Vk  ^  p(^) 

go  to  1 


In  summary,  then,  the  correction  is  computed  from 

JA6»  =  At 

with  At  defined  as 

At=  rvect(Qc?Q^^) 

L 

and  Ap  defined,  in  turn,  as  the  difference  between  the  prescribed  value  of  the 
position  vector  of  the  operation  point  and  its  value  p^  at  the  current  iteration.  Thus, 
the  numerical  path-tracking  scheme  consists  essentially  of  Eqs.  (11.79)  and  (11.80), 
as  first  proposed  by  Pieper  (1968).  We  thus  have  Algorithm  11.5.2. 

When  implementing  the  foregoing  procedure,  we  want  to  save  processing  time; 
hence,  we  aim  at  fast  computations.  The  computation  of  the  correction  AO  involves 
only  linear-equation  solving,  which  was  discussed  at  length  in  Chap.  4  and  need 
not  be  discussed  further  here.  The  only  item  that  still  needs  some  discussion  is  the 
calculation  of  the  vector  norm  ||  A^  ||.  Since  any  norm  can  be  used  here,  we  can 
choose  the  norm  that  is  fastest  to  compute,  namely,  the  maximum  norm,  also  known 
as  the  Chebyshev  norm,  represented  as  ||  A^  ||oo,  and  defined  as 

II A^  llcx)  =  niax{  |0;  I  }  (11.81) 

i 

Note  that  this  norm  only  requires  comparisons  and  no  floating-point  operations.  The 
Euclidean  norm  of  an  /7 -dimensional  vector,  however,  requires  n  multiplications, 
n  —  I  additions,  and  one  square  root. 


(11.79) 


(11.80) 
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Example  11.5.1  (Path-Tracking  for  Arc -Welding).  With  reference  to  the 
arc-welding  exercise  of  Example  11.3.2,  we  want  to  use  the  Fanuc  Arc  Mate, 
whose  Denavit-Hartenberg  parameters  are  listed  in  Table  5.2.  Furthermore,  the 
welding  seam  to  be  tracked  is  placed  well  within  the  workspace  of  the  manipulator. 
A  location  found  quite  suitable  for  this  task  was  obtained  with  the  aid  of  RVS,  our 
Robot  Visualization  System.  This  location  requires  that  the  coordinate  frame  IFc  of 
Fig.  11.4  have  its  axes  parallel  pairwise  to  those  of  the  robot  base,  IFi.  The  latter  is 
defined  according  to  the  Denavit-Hartenberg  notation,  and  so  Zi  coincides  with  the 
axis  of  the  first  revolute;  it  is,  moreover,  directed  upwards.  The  position  found  for 
the  origin  Oc  of  Tq  ^  of  position  vector  o,  is  given  in  Ti  as 


"-1.0' 

[0]l  ^ 

T 

— 

-0.1 

_z_ 

_  0.5  _ 

m 


Find  the  time-histories  of  all  the  joint  variables  that  will  perform  the  desired 
operation  with  the  tip  of  the  electrode  traversing  the  intersection  curve  at  the 
constant  speed  of  Vq  =  0.1  m/s.  Furthermore,  plot  the  variation  of  the  condition 
number  of  the  Jacobian  matrix  along  the  path. 

Solution:  The  robot  at  hand  was  studied  in  Sect.  9.7,  where  it  was  found  not  to 
be  of  the  decoupled  type.  In  fact,  this  robot  does  not  admit  a  closed-form  inverse 
displacement  solution,  and  hence,  the  foregoing  iterative  procedure  is  to  be  used. 

At  the  outset,  we  calculate  all  inverse  displacement  solutions  at  the  pose 
corresponding  to  cp  =  0  using  the  bivariate-equation  approach  of  Sect.  9.4.  This 
pose  is  defined  by  the  orthogonal  matrix  Q  and  the  position  vector  p  given  below: 


"0.6030  0  -0.7977" 

"-0.5500" 

[Q]i  =  [e*  Ct  e„]  = 

0  1  0 
_0.7977  0  0.6030  _ 

^  [p]i  = 

-0.100 
_  0.8969  _ 

with  both  Q  and  p  given  in  robot-base  coordinates.  The  contours  for  the  above 
pose,  which  were  obtained  using  the  procedure  of  Sect.  9.4,  are  shown  in  Fig.  11.7, 
the  eight  solutions  obtained  being  summarized  in  Table  11.3,  which  includes  the 
condition  number  of  the  Jacobian,  ^(j),  of  each  solution.  Note  that  the  calculation 
of  /c(J)  required  computing  the  characteristic  length  of  the  robot,  as  explained  in 
Sect.  5.8.  This  length,  as  calculated  in  that  section,  turned  out  to  be  L  =  0.3573  m. 

Now,  we  have  eight  solutions  at  our  disposal,  from  which  we  must  choose  one 
for  path-tracking.  In  the  absence  of  any  criterion  to  single  out  one  specific  solution, 
we  can  pick  up  the  solution  with  the  lowest  condition  number.  If  we  do  this,  we  end 
up  with  solution  1  in  Table  11.3.  However,  when  we  attempted  to  track  the  given 
path  with  this  solution,  it  turned  out  that  this  solution  encountered  a  singularity  and 
was  hence  discarded.  Of  the  seven  remaining  solutions,  solution  5  has  the  lowest 
condition  number;  this  solution  led  to  a  singularity-free  trajectory. 


11.5  Continuous-Path  Tracking 


499 


Fig.  11.7  Contour  solutions 

of  the  Fanuc  Arc  Mate  robot  ,  ^ 

at  the  given  EE  pose 

100 

50 

6>5  (deg)  0 
-50 

-100 

-150 


Table  11.3  Inverse  kinematics  solutions  of  the  Fanuc  Arc  Mate  robot  for  the  given  EE  pose 


i 

'C(J) 

Ox 

Oi 

Oa 

^5 

e. 

1 

4.74 

19.9039° 

124.909° 

-176.484° 

16.1379° 

-102.29° 

-15.8409° 

2 

4.85 

-3.6664° 

124.723° 

-173.071° 

177.019° 

o 

kO 

o 

-177.208° 

3 

11.12 

-154.951° 

-67.5689° 

-135.549° 

141.716° 

146.966° 

17.754° 

4 

6.31 

-176.328° 

-63.4487° 

-129.817° 

-4.5893° 

-140.319° 

-178.681° 

5 

4.79 

-176.341° 

75.1632° 

-76.6692° 

3.7343° 

51.4104° 

-179.877° 

6 

5.20 

-153.567° 

73.4546° 

-72.5407° 

-153.868° 

-53.7328° 

-0.5046° 

7 

8.68 

-3.6362° 

-129.644° 

-32.9672° 

-175.011° 

-144.428° 

178.133° 

8 

9.94 

18.9031° 

-131.096° 

-26.8084° 

-28.6793° 

147.417° 

13.0786° 

Once  the  appropriate  solution  is  chosen,  the  trajectory  can  be  tracked  with  the  aid 
of  Algorithm  11.5.2.  Here,  we  need  a  discrete  set  of  poses  at  equal  time-intervals. 
Note  that  we  can  produce  such  a  set  at  equal  intervals  of  angle  cp  because  we  have 
expressions  for  the  pose  variables  in  terms  of  this  angle.  In  order  to  obtain  this  set 
at  equal  time-intervals,  then,  we  need  angle  ^  as  a  function  of  time,  i.e.,  (pit).  In  the 
sequel,  we  will  also  need  the  time  T  required  to  complete  the  task.  Now,  since  the 
speed  of  the  electrode  tip  is  constant  and  equal  to  vq,  the  time  T  is  readily  obtained 
by  dividing  the  total  length  /  of  the  curve  by  Vq.  The  length  of  the  curve,  in  turn,  can 
be  computed  as  ^(27r),  where  function  sicp)  denotes  the  arc  length  as  a  function  of 
angle  (p,  i.e.. 


(11.82) 
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We  thus  obtain,  by  numerical  quadrature, 

/  =  silit)  =  1.0257  m 


Hence,  the  total  time  is 


/ 

T  =  —  =  10.257  s 
Vo 

Now,  in  order  to  obtain  ^(t),  we  first  calculate  s  as 

,  _  ds  ds  d(p  .  ds 


dt  d(p  dt 


d(p 


(11.83a) 


Furthermore,  we  note  that  ds/dcp  =  ||r'(^)||,  which  allows  us  to  write  s  as 

i  =  <p\W{(p)\\ 

Moreover,  ||r'(^)  ||  was  found  in  Eq.  (1 1.39b)  to  be 

||r'((p)||  =  rG{(p) 


s  thus  becoming 

s  =  rG(p  (11.83b) 

Furthermore,  we  recall  the  expression  derived  for  G((p)  in  Eq.  (11.39e).  This 
expression,  along  with  the  constancy  condition  on  i,  i.e.,  s  =  Vq,  leads  to 

r(p^/l  {Xcp  micpY  =  Vo 

where  r  is  the  radius  of  the  cylinder.  Upon  solving  for  cp  from  the  above  equation, 
we  obtain 


—  2X  cos  (p 

p^  —  2X  cos  (p  X'^  sin^  (p 

which  is  a  nonlinear  first-order  differential  equation  for  (pit).  Its  initial  value  can 
be  assigned  as  (^(0)  =  0,  thereby  formulating  a  nonlinear  first-order  initial- value 
problem.  The  numerical  solution  of  the  foregoing  problem  is  nowadays  routine 
work,  which  can  be  handled  with  suitable  software,  e.g.,  Matlab  (Hanselman  and 
Littlefield  2001).  Upon  solving  this  equation,  a  data  file  is  produced  that  contains  the 
time-history  of  cp.  The  plot  of  cp  vs.  nondimensional  time  is  displayed  in  Fig.  11.8a. 
Since  the  variations  of  (pit)  are  relatively  small,  this  plot  provides  little  information 
on  the  time-history  of  interest.  A  more  informative  plot,  that  of  ^(^),  is  included  in 
Fig.  11.8b  for  this  reason.  Apparently,  (p  turns  out  to  be  the  sum  of  a  linear  and  a 
periodic  term. 
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Fig.  11.8  Plot  of  (p  vs.  nondimensional  time 


With  (pit)  known  as  a  function  of  time,  we  can  now  specify  the  pose  of  the  end- 
effector,  i.e.,  p  and  Q,  as  functions  of  time. 

The  whole  trajectory  was  tracked  with  the  robot  at  hand  using  the  algorithm 
outlined  in  this  section.  With  the  aid  of  this  algorithm,  we  produced  the  plots 
of  Fig.  11.9.  Also,  the  time-history  of  the  condition  number  of  the  manipulator 
Jacobian  was  computed  and  plotted  in  Fig.  1 1.10.  Apparently,  the  condition  number 
of  the  Jacobian  remains  within  the  same  order  of  magnitude  throughout  the  whole 
operation,  below  10,  thereby  showing  that  the  manipulator  remains  far  enough  from 
singularities  during  this  task — the  condition  number  becomes  very  large  when  a 
singularity  is  approached,  becoming  unbounded  at  singularities.  A  rendering  of  the 
welding  seam  with  the  Frenet-Serret  triad  at  a  sample  of  points  is  displayed  in 
Fig.  1 1 . 1 1 .  It  is  noteworthy  that  the  torsion  of  the  path  is  manifested  in  this  figure  by 
virtue  of  the  inclination  of  the  Z  axis,  which  changes  from  point  to  point.  In  a  planar 
curve,  this  axis  would  remain  at  a  fixed  orientation  while  traversing  the  curve. 


11.6  Exercises 

11.1  A  PUMA  560  robot,  with  the  DH  parameters  of  Table  1 1 .4,  is  used  to  perform 
a  gluing  operation  as  indicated  below:  A  nozzle  dispensing  the  glue  is  rigidly 
attached  to  the  gripper  of  the  robot.  The  tip  of  the  nozzle,  point  P ,  is  to  trace 
a  helicoidal  path  at  a  constant  rate  of  50mm/s.  Furthermore,  the  center  of  the 
wrist  is  located  at  a  point  C ,  fixed  to  a  Frenet-Serret  coordinate  frame.  In  this 
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Fig.  11.9  Time-histories  of  the  joint  variables  (in  degrees)  of  the  Fanuc  Arc  Mate  robot  used  to 
track  a  warped  curve  for  arc-welding  vs.  nondimensional  time 


Fig.  11.10  Time-history  of 
the  condition  number  of  the 
Jacobian  matrix  during  an 
arc- welding  operation  vs. 
nondimensional  time 


frame,  the  coordinates  of  C  are  (0,  —50,  86.7)  mm.  Moreover,  the  path  to 
be  traced  by  point  P  is  given  as 

X  =  a  cos  1^,  y  =  a  sim^,  z  =  0  <  ?^  <  7r/2 

with  the  values  a  =  300  mm,  b  =  SOO/tt  mm. 

(a)  Decide  where  to  locate  the  robot  base  with  respect  to  the  path  so  that  the  latter 
will  lie  well  within  the  workspace  of  the  robot.  Then,  produce  plots  of  Ot  vs. 
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Fig.  11.11  Welding  seam 
with  Frenet-Serret  frames 


Table  11.4  DH  parameters 
of  a  PUMA  560  robot 


ii  'Zi 


X 


Joint  i 

Oii  (°) 

Qi  (m) 

hi  (m) 

1 

90 

0 

0.660 

2 

0 

0.432 

0 

3 

90 

0.020 

0.149 

4 

90 

0 

0.432 

5 

90 

0 

0 

6 

0 

0 

0.056 

t,  for  0  <  t  <  T,  where  T  is  the  time  it  takes  to  traverse  the  whole  trajectory, 
for  /  =  1, 2, . . . ,  6. 

(b)  Produce  plots  of  Of  vs.  ^  in  the  same  time  interval  for  all  six  joints. 

•  • 

(c)  Produce  plots  of  Of  vs.  ^  in  the  same  time  interval  for  all  six  joints. 

11.2  A  bracket  for  spot- welding,  shown  in  Fig.  11.12,  is  rigidly  attached  to  the 
end-effector  of  a  robotic  manipulator.  It  is  desired  that  point  P  of  the  bracket 
follow  a  helicoidal  path  F,  while  keeping  the  orientation  of  the  bracket  with 
respect  to  F  as  indicated  below:  Let  B  =  {io,jo,ko}  and  X  =  {i7,j7,k7} 
be  triads  of  unit  orthogonal  vectors  fixed  to  the  base  of  the  robot  and  to  the 
bracket,  respectively.  Moreover,  let  T  =  {e^ ,  e„ ,  }  be  the  Frenet-Serret  triad 

of  F,  given  as 
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Fig.  11.12  A  bracket  for  spot- welding 


=  — 0.6sin^io  +  0.6  cos  (^jo  +  0.8ko 
e„  =  -  cos  (p\o  -  sin  (^jo 

=0.8  sin  ^io  —  0.8  cos  ^jo  +  0.6ko 

where  is  a  given  function  of  time,  (pit). 

Furthermore,  the  orientation  of  the  bracket  with  respect  to  F  is  to  be  kept 
constant  and  given  in  terms  of  the  Frenet-Serret  triad  as 

=  0.933e^  +  0.067e„  —  0.354e^ 

=  0.067e^  +  0.933e„  +  0.354e^ 
kv  =  0.354e^  —  0.354e„  +  0.866e^ 

Additionally,  R  and  S(0  denote  the  rotation  matrices  defining  the  orienta¬ 
tion  of  T-j  with  respect  to  T  and  of  T  with  respect  to  S,  respectively. 

(a)  Find  the  matrix  representation  of  S(0  in  B. 

(b)  Find  the  matrix  representation  of  R  in  T. 

(c)  Let  Q(0  denote  the  orientation  of  T']  with  respect  to  B.  Find  its  matrix 
representation  in  B. 
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(d)  Find  the  Darboux  vector  8  of  the  path,  along  with  its  time-derivative,  8,  in 
base-fixed  coordinates.  Note:  You  can  do  this  in  several  ways,  as  discussed 
in  Sect.  11.2.  Choose  the  one  that  will  allow  you  to  use  previously  computed 
results,  thereby  simplifying  the  computations. 

1 1.3  The  parametric  equations  of  a  curve  are  given  as 

X  =  2t,  y  =  t^,  z  =  t^ /2 

where  t  is  time.  A  robotic  manipulator  is  to  follow  this  trajectory  so  that  its 
gripper  keeps  a  constant  orientation  with  respect  to  the  Frenet-Serret  frame 
of  the  curve. 

(a)  Determine  the  unit  vector  parallel  to  the  axis  of  rotation  and  the  angle  of 
rotation  of  the  gripper  as  functions  of  time. 

(b)  Find  the  angular  velocity  and  angular  acceleration  of  the  gripper  as  functions 
of  time. 

11.4  Derive  Eqs.  (11.45a  and  b). 

11.5  Find  the  spline  approximation  of  the  helix  of  Example  11.3.1.  Then,  plot  the 
approximation  errors  of  the  Cartesian  coordinates  of  points  of  the  helix,  for 
N  =  5,  11,  and  21  equally  spaced  supporting  points.  In  order  to  assess  the 
orientation  error,  compute  the  Darboux  vectors  of  the  spline,  8s,  and  of  the 
helix,  8h  -  The  approximation  error  of  the  orientation  is  now  defined  as 

Co  =  max{||5^(^)  -  Sh((p)\\} 

(P 

with  (p  defined  as  in  Example  11.3.1. 

11.6  Find  the  spline  approximation  of  the  curvature,  torsion,  and  Darboux  vector 
of  the  curve  introduced  in  Example  11.3.2.  Find  expressions  for  the  exact 
values  of  these  variables  and  plot  the  approximation  errors,  for  5,10,  and  20 
equally  spaced  supporting  points  vs.  (p.  In  the  error  definitions  given  below, 
subscript  e  indicates  exact  value,  subscript  ^  spline  value: 

eK  =  Ks{(p)  -  Ke{(p) 

Sz  =  Ts  (V)  -  re  ((p) 

11.7  From  the  plots  of  the  time-histories  of  the  joint  angles  calculated  in  Exam¬ 
ple  11.5.1,  it  is  apparent  that,  with  the  exception  of  O4,  which  has  a  linear 
component,  these  histories  are  periodic.  Repeat  Example  11.5.1,  but  now 
using  a  spline  approximation  of  the  welding  seam,  with  N  =  5,  10,  and 
20  supporting  points.  With  this  spline  approximation,  calculate  the  pose,  the 
twist,  and  the  twist-rate  at  each  supporting  point.  Now,  calculate  values  of  0 , 
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•  •  • 

0 ,  and  0  at  each  of  these  supporting  points  by  means  of  inverse  kinematics. 

•  • 

Compare  the  values  thus  obtained  of  0  with  those  derived  from  the  linear 
relation  between  the  function  values  and  the  values  of  its  second  derivative  at 
the  supporting  points  when  using  a  cubic  spline. 

11.8  The  decoupled  robot  of  Fig.  4.23  is  to  perform  an  arc-welding  operation  along 
a  welding  seam  that  requires  its  wrist  center  C  to  travel  at  a  constant  speed 
of  1  m/s  along  a  line  joining  points  A  and  B,  not  shown  in  that  figure,  while 
keeping  the  EE  holding  the  electrode  at  a  constant  orientation  with  respect 
to  the  base  frame.  Moreover,  the  seam  is  to  be  traversed  according  to  the 
following  schedule:  With  point  C  located  at  a  point  A'  on  the  extension  of 
AB,  Si  distance  of  250mm  from  A,  point  C  approaches  A  with  a  cycloidal 
motion  at  the  specified  speed;  upon  reaching  B,  point  C  decelerates  with  a 
cycloidal  motion  as  well,  until  it  reaches  a  point  B'  in  the  other  extension  of 
AB,  250  mm  from  B,  with  zero  speed.  The  position  vectors  of  points  A  and 
B,  denoted  by  a  and  b,  respectively,  are  given,  in  base  coordinates,  as 


"  500  ■ 

"1,200' 

a  = 

-500 

,  b  = 

0 

_  500  _ 

_1,200_ 

in  mm.  For  the  above-given  data,  find  the  time-histories  of  all  joint  variables. 

1 1 .9  Derive  expressions  (1 1 .45a  and  b). 

1 1.10  If  linear  invariants  are  used  to  represent  the  desired  pose  Sj,  then  q  =  0  and 
^0  =  1  when  the  angle  of  rotation  becomes  tt  .  Under  these  conditions,  matrix 
T  of  Eq.  (11.72)  becomes  rank-deficient,  this  equation  thus  not  necessarily 
leading  to  Eq.  (11.73).  One  way  of  coping  with  this  algorithmic  singularity 
consists  in  redefining  axis  Xi  of  the  DH  notation  by  rotating  the  current  Xi 
axis  by  an  angle  A^i  about  Zi,  which  does  not  affect  the  remaining  variables 
and  parameters  of  the  said  notation. 

Find  the  optimum  value  of  A^i  that  will  take  T  “farthest”  from  its  current 
rank-deficiency. 


Chapter  12 

Dynamics  of  Complex  Robotic  Mechanical 
Systems 


12.1  Introduction 

The  subject  of  this  chapter  is  the  dynamics  of  the  class  of  robotic  mechanical 
systems  introduced  in  Chap.  10  under  the  generic  name  of  complex.  Notice  that 
this  class  comprises  serial  manipulators  not  allowing  a  decoupling  of  the  orientation 
from  the  positioning  tasks.  For  purposes  of  dynamics,  this  decoupling  is  irrelevant 
and  hence,  was  not  a  condition  in  the  study  of  the  dynamics  of  serial  manipulators  in 
Chap.  7.  Thus,  serial  manipulators  need  not  be  further  studied  here,  the  focus  being 
on  parallel  manipulators  and  rolling  robots.  The  dynamics  of  walking  machines  and 
multifingered  hands  involves  special  features  that  render  these  systems  more  elab¬ 
orate  from  the  dynamics  viewpoint,  for  they  exhibit  a  time-varying  topology.  What 
this  means  is  that  these  systems  include  kinematic  loops  that  open  when  a  leg  takes 
off  or  when  a  finger  releases  an  object  and  open  chains  that  close  when  a  leg  touches 
ground  or  when  a  finger  makes  contact  with  an  object.  The  implication  here  is  that 
the  degree  of  freedom  of  these  systems  is  time- varying.  The  derivation  of  such  a 
mathematical  model  is  discussed  in  Pfeiffer  et  al.  (1995),  but  is  left  out  in  this  book. 

The  degree  of  freedom  (dof)  of  the  mechanical  systems  studied  here  is  thus 
constant.  Now,  the  two  kinds  of  systems  studied  here  pertain  to  very  different  types, 
for  parallel  manipulators  fall  into  the  realm  of  holonomic,  while  rolling  robots 
into  that  of  nonholonomic,  mechanical  systems.  In  order  to  better  understand  this 
essential  difference  between  these  two  types  of  systems,  we  give  below  a  summary 
of  the  classification  of  mechanical  systems  at  large. 

12.2  Classification  of  Robotic  Mechanical  Systems 
with  Regard  to  Dynamics 

Because  robotic  mechanical  systems  are  a  class  of  general  mechanical  systems, 
a  classification  of  the  latter  will  help  us  focus  on  the  systems  motivating  this 
study.  Mechanical  systems  can  be  classified  according  to  various  criteria,  the  most 
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common  one  being  based  on  the  type  of  constraints  to  which  these  systems  are 
subjected.  In  this  context  we  find  holonomic  vs.  nonholonomic  and  scleronomic  vs. 
rheonomic  constraints.  Holonomic  constraints  are  those  that  are  expressed  either 
as  a  system  of  algebraic  equations  in  displacement  variables,  whether  angular  or 
translational,  not  involving  any  velocity  variables,  or  as  a  system  of  equations  in 
velocity  variables  that  nevertheless  can  be  integrated  as  a  whole  to  produce  a  system 
of  equations  of  the  first  type.  Note  that  it  is  not  necessary  that  every  single  scalar 
equation  of  velocity  constraints  be  integrable;  rather,  the  whole  system  must  be 
integrable  for  the  system  of  velocity  constraints  to  lead  to  a  system  of  displacement 
constraints.  If  the  system  of  velocity  constraints  is  not  integrable,  the  constraints 
are  said  to  be  nonholonomic.  Moreover,  if  a  mechanical  system  is  subject  only  to 
holonomic  constraints,  it  is  said  to  be  holonomic;  otherwise,  it  is  nonholonomic. 
Manipulators  composed  of  revolute  and  prismatic  pairs  are  examples  of  holonomic 
systems,  while  wheeled  robots  are  usually  nonholonomic  systems.  On  the  other 
hand,  if  a  mechanical  system  is  subject  to  constraints  that  are  not  explicit  functions 
of  time,  these  constraints  are  termed  scleronomic,  while  if  the  constraints  are  explicit 
functions  of  time,  they  are  termed  rheonomic.  For  our  purposes,  however,  this 
distinction  is  irrelevant. 

In  order  to  understand  better  one  more  classification  of  mechanical  systems, 
we  recall  the  concepts  of  generalized  coordinate  and  generalized  speed  that  were 
introduced  in  Sect.  7.3.2.  The  generalized  coordinates  of  a  mechanical  system  are 
all  those  displacement  variables,  whether  rotational  or  translational,  that  determine 
uniquely  a  configuration  of  the  system.  Note  that  the  set  of  generalized  coordinates 
of  a  system  is  not  unique.  Moreover,  various  sets  of  generalized  coordinates  of 
a  mechanical  system  need  not  have  the  same  number  of  elements,  but  there  is  a 
minimum  number  below  which  the  set  of  generalized  coordinates  cannot  define 
the  configuration  of  the  system.  This  minimum  number  corresponds,  in  the  case 
of  holonomic  systems,  to  the  degree  of  freedom  of  the  system.  Serial  and  parallel 
manipulators  coupled  only  by  revolute  or  prismatic  pairs  are  holonomic,  their  joint 
variables,  grouped  in  vector  0 ,  playing  the  role  of  generalized  coordinates,  while 
their  joint  rates,  grouped  in  vector  0,  in  turn,  play  the  role  of  generalized  speeds. 
Note  that  in  the  case  of  parallel  manipulators,  not  all  joint  variables  are  independent 
generalized  coordinates.  In  the  case  of  nonholonomic  systems,  on  the  other  hand, 
the  number  of  generalized  coordinates  needed  to  fully  specify  their  configuration 
exceeds  their  degree  of  freedom  by  virtue  of  the  lack  of  integrability  of  their 
kinematic  constraints.  This  concept  is  best  illustrated  with  the  aid  of  examples, 
which  are  included  in  Sect.  12.5.  Time-derivatives  of  the  generalized  coordinates, 
or  linear  combinations  thereof,  are  termed  the  generalized  speeds  of  the  system. 
If  the  kinetic  energy  of  a  mechanical  system  is  zero  when  all  its  generalized  speeds 
are  set  equal  to  zero,  the  system  is  said  to  be  catastatic.  If,  on  the  contrary,  the 
kinetic  energy  of  the  system  is  nonzero  even  if  all  the  generalized  speeds  are  set 
equal  to  zero,  the  system  is  said  to  be  acatastatic.  All  the  systems  that  we  will  study 
in  this  chapter  are  catastatic.  A  light  robot  mounted  on  a  heavy  noninertial  base  that 
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undergoes  a  controlled  motion  is  an  example  of  an  acatastatic  system,  for  the  motion 
of  the  base  can  be  assumed  to  be  insensitive  to  the  dynamics  of  the  robot;  however, 
the  motion  of  the  base  does  affect  the  dynamics  of  the  robot. 

Another  criterion  used  in  classifying  mechanical  systems,  which  pertains  specif¬ 
ically  to  robotic  mechanical  systems,  is  based  on  the  type  of  actuation.  In  general, 
a  system  needs  at  least  as  many  independent  actuators  as  degrees  of  freedom. 
However,  instances  arise  in  which  the  number  of  actuators  is  greater  than  the  degree 
of  freedom  of  the  system.  In  these  instances,  we  speak  of  redundantly  actuated 
systems.  In  view  of  the  fundamental  character  of  this  book,  we  will  not  study 
redundant  actuation  here;  we  will  thus  assume  that  the  number  of  independent 
actuators  equals  the  degree  of  freedom  of  the  system. 

The  main  results  of  this  chapter  are  applicable  to  robotic  mechanical  systems 
at  large.  For  brevity,  we  will  frequently  refer  to  the  objects  of  our  study  simply  as 
systems. 


12.3  The  Structure  of  the  Dynamics  Models 
of  Holonomic  Systems 

We  saw  in  Sect.  7.6  that  the  mathematical  model  of  a  manipulator  of  the  serial 
type  contains  basically  three  terms,  namely,  one  linear  in  the  joint  accelerations, 
one  quadratic  in  the  joint  rates,  and  one  arising  from  the  environment,  i.e.,  from 
actuators,  dissipation,  and  potential  fields  such  as  gravity.  We  show  in  this  section 
that  in  fact,  the  essential  structure  of  this  model  still  holds  in  the  case  of  more  general 
mechanical  systems  subject  to  holonomic  constraints,  if  we  regard  the  rates  of  the 
actuated  joints  as  the  independent  generalized  speeds  of  the  system.  Nonholonomic 
robotic  systems  are  studied  in  Sect.  12.5. 

First,  we  will  assume  that  the  mechanical  system  at  hand  is  composed  of  r  rigid 
bodies  and  its  degree  of  freedom  is  n.  Henceforth,  we  assume  that  these  bodies  are 
coupled  in  such  a  way  that  they  may  form  kinematic  loops;  for  this  reason,  such 
systems  contain  some  unactuated  joints.  Definitions  similar  to  those  of  Sect.  7.3.1 
are  henceforth  adopted.  In  this  vein,  the  manipulator  mass  matrix  of  that  section 
becomes  now,  more  generically,  the  6r  x  6r  system  mass  matrix  M,  the  6r  x  6r 
system  angular  velocity  matrix  W,  and  the  6 r -dimensional  system  twist  vector  t 
being  defined  likewise. 

We  assume  further  that  the  total  number  of  joints,  active  and  passive,  is  m  >  /7. 
The  m -dimensional  array  0  of  joint  variables,  associated  with  both  actuated  and 
unactuated  joints,  is  thus  naturally  partitioned  into  two  subarrays,  the  -dimensional 
vector  of  actuated  joint  variables  Oa  and  its  m' -dimensional  unactuated  counterpart 
Ou,  with  m'  =  m  —  n,  namely. 
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We  can  now  set  up  the  mathematical  model  of  the  system  at  hand  using  the 
natural  orthogonal  complement,  as  introduced  in  Sect.  7.5.  Since  the  system  under 
study  has  n  degrees  of  freedom,  the  model  sought  must  be  a  system  of  n  second- 
order  ordinary  differential  equations.  We  can  proceed  to  derive  this  model  as  we  did 
in  Sect.  7.5,  by  regarding  all  joints  first  as  if  they  were  independent,  but  taking  into 
account  that  only  n  of  the  total  m  joints  are  actuated.  We  do  this  by  introducing 
a  vector  of  constraint  forces,  as  is  done  in  the  realm  of  Lagrangian  dynamics 
(Torok  2000).  In  this  vein,  we  first  represent  the  twists  of  all  the  moving  links  as 
linear  transformations  of  the  joint-rate  vector  0 ,  then  assemble  all  the  individual 
six-dimensional  twist  arrays  into  the  6  r -dimensional  array  t  defined  above  as  the 
system  twist.  We  thus  end  up  with  a  relation  of  the  form 

t  =  V(0)0  (12.2) 

where  U(^)  is  the  6r  x  m  twist- shaping  matrix,  playing  a  role  similar  to  that  of 
matrix  T  of  Sect.  7.5.  Moreover,  the  constraints  relating  all  joint  rates  can  be  cast  in 
the  form 


A(0)0  =  Op  (12.3) 

where  A(^)  is  a  pxm  matrix,  whereby  p  <  m,  with  nullity — the  nullity  of  a  matrix 
is  the  dimension  of  its  null  space — v  =  n,  and  0^  is  the  -dimensional  zero  vector. 
Given  the  nullity  of  A(^),  up  to  n  of  the  m  components  of  0  can  be  assigned  freely 
without  violating  the  constraints  (12.3),  which  is  compatible  with  the  assumption 
on  the  dof  of  the  system.  Note  that,  in  setting  up  the  foregoing  p  constraints  on  the 
joint  rates,  the  number  p  depends  on  the  topology  of  the  system,  i.e.,  on  its  number 
of  links;  on  its  number  of  joints;  and  on  how  the  links  are  coupled,  so  as  to  form 
kinematic  loops. 

In  applying  the  procedure  of  the  natural  orthogonal  complement  to  the  con¬ 
strained  system,  we  end  up  with  a  system  of  m  second-order  ordinary  differential 
equations,  namely,  the  Euler-Lagrange  equations  of  a  system  constrained  by  the 
relations  (12.3),  which  thus  takes  the  form 


\0  +C{0,0)e  =  r  +  8  +  y  +Vy/^  +  pjx  (12.4a) 

The  above  equation  contains  terms  that  are  familiar  from  Sect.  7.5,  except  for  the  last 
term  of  the  right-hand  side.  This  term  accounts  for  the  generically  termed  constraint 
forces  and  amounting  to  constraint  joint  torques  and  forces  that  must  be  exerted  at 
all  joints  in  order  to  maintain  the  topology  of  the  system.  Vector  A  is  termed  the 
vector  of  Lagrange  multipliers  in  the  realm  of  Lagrangian  dynamics.  In  the  above 
equation,  the  definitions  below,  similar  to  those  of  Eqs.  (7.58)  and  (7.59),  have  been 
introduced: 
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i(6l)  =  U^MU  (12.4b) 

Ciej)  =  U^MU  +  WMU  (12.4c) 

i  =  8  =  U^w^,  y  =  (12.4d) 


Moreover,  w'^,  w^,  w*^,  and  are  the  various  types  of  wrenches  acting  on  the 
system:  exerted  by  the  actuators;  stemming  from  dissipation  effects;  due  to  the 
gravity  field;  and  exerted  by  the  environment,  respectively.  In  turn,  J  is  the  6  x  m 
Jacobian  matrix  mapping  the  system  joint  rates  into  the  end-effector  twist,  while 
is  assumed  applied  onto  the  end-effector. 

Upon  resorting  to  the  kinematics  of  the  system,  it  is  possible  to  express  the  vector 

•  • 

of  joint  rates  ^  as  a  linear  transformation  of  the  vector  of  actuated  joint  rates  0 a, 
namely,^ 


0  =  S(0a)K  (12.5) 

where  we  have  assumed  that,  from  the  geometry  of  the  system,  0  ^  has  been  solved 
for  in  terms  of  Oa  -  Further,  upon  substitution  of  Eq.  (12.5)  into  Eq.  (12.3),  we  obtain 

A(0)e(0a)K  =  ()p 

which  must  hold  for  any  0  a ,  given  the  dof  of  the  system.  As  a  consequence,  then, 

A(0)S(0a)=Opn  (12.6) 

and  hence,  is  an  orthogonal  complement  of  A(^),  which  we  can  also  call 

a  natural  orthogonal  complement.  Notice,  however,  that  contrary  to  the  natural 
orthogonal  complement  U,  which  maps  the  joint-rate  vector  onto  the  system  twist, 
0  maps  the  space  of  actuated  joint  rates  into  that  of  the  system  joint  rates. 
Apparently, 


0  =  e(0a)0a^e(0a,0a)0a  (12.7) 

Upon  substitution  of  Eq.  (12.7)  into  Eq.  (12.4a),  we  obtain 


i^Oa  +  i0^fl  +  C(^a,  0a)^0a  —  T  +  5  +  }/  +  +  A^A 

Further,  the  term  of  constraint  forces  is  eliminated  from  the  above  equations  upon 

'T' 

multiplying  both  sides  of  the  above  equation  by  0  from  the  left,  thus  obtaining 
the  mathematical  model  sought,  i.e.. 


lO^+Cea  =  r+8  +  y+j'^w'^  (12.8a) 


10 

is  not  to  be  confused  with  the  matrix  defined  in  Eqs.  (10.54a  and  b). 
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with  the  definitions  below: 

I  =  T^MT,  C  =  T^MT  +  T^WMT,  J  =  J0, 

T  =  S  =  &^S,  y  =  &^y. 


(12.8b) 

(12.8c) 


and 


T  =  U0  (12.8d) 

That  is,  the  mathematical  model  governing  the  dynamics  of  any  holonomic  robotic 
mechanical  system  is  formally  identical  to  that  of  Eq.  (7.61)  obtained  for  serial 
manipulators. 


12.4  Dynamics  of  Parallel  Manipulators 

We  illustrate  the  modeling  techniques  of  mechanical  systems  with  kinematic  loops 
via  a  class  of  systems  known  as  parallel  manipulators.  While  parallel  manipulators 
can  take  on  a  large  variety  of  forms,  we  focus  here  on  those  termed  platform 
manipulators,  with  an  architecture  similar  to  that  of  flight  simulators.  In  platform 
manipulators  we  can  distinguish  two  special  links,  namely,  the  base  B  and  the 
moving  platform  M..  Moreover,  these  two  links  are  coupled  via  six  legs,  with 
each  leg  constituting  a  six-axis  kinematic  chain  of  the  serial  type,  as  shown  in 
Fig.  12.1,  whereby  a  wrench  w^,  represented  by  a  double-headed  arrow,  acts  on 
M.  and  is  applied  at  Cm^  the  mass  center  of  M.  This  figure  shows  the  axes  of 
the  revolutes  coupling  the  legs  to  the  two  platforms  as  forming  regular  polygons. 
However,  the  modeling  discussed  below  is  not  restricted  to  this  particular  geometry. 
As  a  matter  of  fact,  these  axes  need  not  even  be  coplanar.  On  the  other  hand, 
the  architecture  of  Fig.  12.1  is  very  general,  for  it  includes  more  specific  types  of 
platform  manipulators,  such  as  flight  simulators.  In  these,  the  first  three  revolute 
axes  stemming  from  the  base  platform  have  intersecting  axes,  thereby  giving  rise  to 
a  spherical  kinematic  pair,  while  the  upper  two  axes  intersect  at  right  angles,  thus 
constituting  a  universal  joint.  Moreover,  the  intermediate  joint  in  flight  simulators  is 
not  a  revolute,  but  rather  a  prismatic  pair,  which  is  the  actuated  joint  of  the  leg.  A  leg 
kinematically  equivalent  to  that  of  flight  simulators  can  be  obtained  from  that  of 
the  manipulator  of  Fig.  12.1,  if  the  intermediate  revolute  has  an  axis  perpendicular 
to  the  line  connecting  the  centers  of  the  spherical  and  the  universal  joints  of  the 
corresponding  leg,  as  shown  in  Fig.  12.2.  In  flight  simulators,  the  pose  of  the  moving 
platform  is  controlled  by  hydraulic  actuators  that  vary  the  distance  between  these 
two  centers.  In  the  revolute-coupled  equivalent  leg,  the  length  of  the  same  line  is 
controlled  by  the  rotation  of  the  intermediate  revolute. 
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Fig.  12.1  A  platform-type 
parallel  manipulator 


Fig.  12.2  A  leg  of  a  simple 
platform-type  parallel 
manipulator 


Shown  in  Fig.  12.3  is  the  graph  of  the  system  depicted  in  Fig.  12.1.  In  that  graph, 
the  nodes  denote  rigid  links,  while  the  edges  denote  joints.  By  application  of  Euler’s 
formula  for  graphs  (Harary  1972),  the  number  l  of  independent  loops  of  a  system 
with  many  kinematic  loops  is  given  by 
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Fig.  12.3  The  graph  of  the 
flight  simulator 


where  j  is  the  number  of  revolute  and  prismatic  joints  and  /  is  the  number  of  links. 

Thus,  if  we  apply  Euler’s  formula  to  the  system  of  Fig.  12.1,  we  conclude 
that  its  kinematic  chain  contains  five  independent  loops.  Hence,  while  the  chain 
apparently  contains  six  distinct  loops,  only  five  of  these  are  independent.  Moreover, 
the  degree  of  freedom  of  the  manipulator  is  six.  Indeed,  the  total  number  of  links 
of  the  manipulator  is  /  =  6  x  5  +  2  =  32.  Of  these,  one  is  fixed,  and  hence,  we 
have  31  moving  links,  each  with  six  degrees  of  freedom  prior  to  coupling.  Thus,  we 
have  a  total  of  31  x  6  =  186  degrees  of  freedom  at  our  disposal.  Upon  coupling, 
each  revolute  removes  five  degrees  of  freedom,  and  hence,  the  36  kinematic  pairs 
remove  1 80  degrees  of  freedom,  the  manipulator  thus  being  left  with  6  degrees  of 
freedom.  We  derive  below  the  mathematical  model  governing  the  motion  of  the 
overall  system  in  terms  of  the  independent  generalized  coordinates  associated  with 
the  actuated  joints  of  the  legs. 

We  assume,  henceforth,  that  each  leg  is  a  six-axis  open  kinematic  chain  with 
either  revolute  or  prismatic  pairs,  only  one  of  which  is  actuated,  and  we  thus  have 
as  many  actuated  joints  as  degrees  of  freedom.  Furthermore,  we  label  the  legs  with 
Roman  numerals  /,//,...,  U/  and  denote  the  mass  center  of  the  mobile  platform 
M.  by  Cx,  with  the  twist  of  M.  denoted  by  Im  and  defined  at  the  mass  center.  That 
is,  if  Cm  denotes  the  position  vector  of  Cm  in  an  inertial  frame  and  Cm  its  velocity, 
while  0)^  is  the  angular  velocity  of  M.,  then 
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Fig.  12.4  The  free-body 
diagram  of  A4 


Cm 


(12.10) 


Next,  the  Newton-Euler  equations  of  A4  are  derived  from  the  free-body  diagram 
shown  in  Fig.  12.4.  In  this  figure,  the  legs  have  been  replaced  by  the  constraint 
wrenches  acting  at  point  Cm^  the  governing  equation  thus  taking  the  form 

of  Eq.  (7.5c),  namely. 


VI 

^  Wy  (12.11) 

j=i 

with  denoting  the  external  wrench  acting  on  A4.  Furthermore,  let  us  denote  by 
q  j  the  variable  of  the  actuated  joint  of  the  /th  leg,  all  variables  of  the  six  actuated 
joints  being  grouped  in  the  six-dimensional  array  q,  i.e., 

T 

q  =  qjj  ...  qyj^  (12.12) 

Now,  we  derive  a  relation  between  the  twist  Im  and  the  active  joint  rates,  qj,  for 

J  =  /,//,...,  E/.  To  this  end,  we  resort  to  Fig.  12.5,  depicting  the  /th  leg  as  a 
serial-type,  six-axis  manipulator,  whose  twist-shape  relations  are  readily  expressed 
as  in  Eq.  (5.9),  namely, 

ijOj  =iM,  /  =  /,//,...,  VI  (12.13) 

where  J/  is  the  6x6  Jacobian  matrix  of  the  /th  leg. 

In  Fig.  12.5,  the  moving  platform  A4  has  been  replaced  by  the  constraint  wrench 
transmitted  by  the  moving  platform  onto  the  end  link  of  the  /th  leg,  — Wy ,  whose 
sign  is  the  opposite  of  that  transmitted  by  this  leg  onto  M.  by  virtue  of  Newton’s 
third  law.  The  dynamics  model  of  the  manipulator  of  Fig.  12.5  then  takes  the  form 

ijej  +  Cjiej,ej)ej  =  rj-VjwS,  j  =  ii,  ....vi  (n.u) 

where  1/  is  the  6x6  inertia  matrix  of  the  manipulator,  while  C/  is  the  matrix 
coefficient  of  the  inertia  terms  that  are  quadratic  in  the  joint  rates.  Moreover,  0  j  and 
T  J  denote  the  six-dimensional  vectors  of  joint  variables  and  joint  torques,  namely. 
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Fig.  12.5  The  serial  manipulator  of  the  Jth  leg 


~9ji' 

0j2 


rj  = 


'^Jk 

0 


0 


(12.15) 


with  subscript  Jk  denoting  in  turn  the  only  actuated  joint  of  the  /th  leg,  namely,  the 
k\h  joint  of  the  leg.  If  we  now  introduce  e jk,  defined  as  a  unit  vector  all  of  whose 
entries  are  zero  except  for  the  kth  entry,  which  is  unity,  then  we  can  write 


T/  =  fjejk  (12.16) 

If  the  actuated  joint  is  prismatic,  as  is  the  case  in  flight  simulators,  then  //  is  a  force; 
if  this  joint  is  a  revolute,  then  //  is  a  torque. 

Now,  since  the  dimension  of  q  coincides  with  the  degree  of  freedom  of  the 
manipulator,  it  is  possible  to  find,  within  the  framework  of  the  natural  orthogonal 
complement,  a  6  x  6  matrix  L  j  mapping  the  vector  of  actuated  joint  rates  q  into  the 
vector  of  /th-leg  joint-rates,  namely, 

Oj=Ljq,  J  =  I,  II,  (12.17) 


The  calculation  of  L/  will  be  illustrated  with  an  example. 

Moreover,  if  the  manipulator  of  Fig.  12.5  is  not  at  a  singular  configuration,  then 
we  can  solve  for  from  Eq.  (12.14),  i.e.. 


12.4  Dynamics  of  Parallel  Manipulators 


517 


=r/{Tj -IjBj -Cjh)  (12.18) 

in  which  the  superscript  —T  stands  for  the  transpose  of  the  inverse,  or  equivalently, 
the  inverse  of  the  transpose,  while  1/  =  lj{0  j)  and  C/  =  Cj{0  j  ,0  j).  Further, 
we  substitute  as  given  by  Eq.  (12.18)  into  Eq.  (12.11),  thereby  obtaining  the 
Newton-Euler  equations  of  the  moving  platform  free  of  constraint  wrenches. 
Additionally,  the  equations  thus  resulting  now  contain  inertia  terms  and  joint  torques 
pertaining  to  the  /th  leg,  namely. 


VI 

^MiM  =  M^MiM  + +  -  VOj  -CjOj)  (12.19) 

J=I 

Still  within  the  framework  of  the  natural  orthogonal  complement,  we  set  up  the 
relation  between  the  twist  Im  and  the  vector  of  actuated  joint  rates  q  as 

iM  =  Tq  (12.20) 

which  upon  differentiation  with  respect  to  time,  yields 

=  Tq  +  tq  (12.21) 

In  the  next  step,  we  substitute  Im  and  its  time-derivative  as  given  by  Eqs.  (12.20 
and  12.21)  into  Eq.  (12.19),  thereby  obtaining 

MA^(Tq  +  tq)  +  WMMMTq 

VI  VI 

+  J237(Vh+Cjh)  =  w”'  +  ^  (12.22) 

J=I  J=I 


Further,  we  recall  relation  (12.17),  which  upon  differentiation  with  respect  to 
time,  yields 

Oj  =Ljq  +  tjq  (12.23) 

Next,  relations  (12.17  and  12.23)  are  substituted  into  Eq.  (12.22),  thereby  obtaining 
the  model  sought  in  terms  only  of  actuated  joint  variables.  After  simplification,  this 
model  takes  the  form 

M^^Tq  +  MMTq  +  W^^M^^Tq 

J  =  VI  VI 

+  J7^(I^Lyq  +  Ijhjii  +  CM)  =  ^  JJ^Ty  (12.24) 

J=I  J=I 


where  now  1/  =  1/  (q)  and  C  j  =  C/(q,q). 

Our  final  step  in  this  formulation  consists  in  deriving  a  reduced  6x6  model 
in  terms  only  of  actuated  joint  variables.  Prior  to  this  step,  we  note  that  from 
Eqs.  (12.13),  (12.17),  and  (12.20), 
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Ly  =  jy^T  (12.25) 

Upon  substitution  of  the  above  relation  into  Eq.  (12.24)  and  multiplication  of  both 
sides  of  Eq.  (12.24)  by  from  the  left,  we  obtain  the  desired  model  in  the  form  of 
Eqs.  (12.8a),  namely, 

VI 

M(q)q  +  N(q,q)q  =  T‘^+ ^Lyry  (12.26) 

J=I 

with  the  6x6  matrices  M(q),  N(q,  q),  and  vector  defined  as 

VI 

M(q)  =  T^Ma^T  +  Y,  LylyLy  (12.27a) 

J=I 

VI 

N(q,  q)  =  T^(MAyt  +  WaiMa^T)  +  ^  C(IyLy  +  CyLy)  (12.27b) 

J=I 

(12.27c) 


Alternatively,  the  foregoing  variables  can  be  expressed  in  a  more  compact  form  that 
will  shed  more  light  on  the  above  model.  To  do  this,  we  define  the  36  x  36  matrices 
I  and  C  as  well  as  the  6  x  36  matrix  L,  the  6x6  matrix  A,  and  the  six-dimensional 
vector  0  as 


I  =  diag(I/,  I//, . . . ,  Ip/)  (12.28a) 

C  =  diag(C/ ,  C// , . . . ,  Cp/)  (12.28b) 

L  =  I^L/  L//  . . .  Lp/]  (12.28c) 

A  =  [L/C/^  L//e//^  . . .  Lp/Cp/^]  (12.28d) 

fii  ■■■  fvif  (12.28e) 

and  hence, 

M(q)  =  +  L^IL  (12.29a) 

N(q,  q)  =  T^(MA^t  +  Wa^MaiT)  +  L^IL  +  L^C(q,  q)L  (12.29b) 

VI 

^Ljry^A^  (12.29c) 


J  =  I 


whence  the  mathematical  model  of  Eq.  (12.26)  takes  on  a  more  familiar  form, 
namely. 
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M(q)q  +  N(q,q)q  =  T^  +  A0  (12.30) 

Thus,  for  inverse  dynamics,  we  want  to  determine  0  for  a  motion  given  by  q  and 
q,  which  can  be  done  from  the  above  equation,  namely, 

^  =  A-‘[M(q)q  +  N(q,q)q-T‘^]  (12.31) 

Notice,  however,  that  the  foregoing  solution  is  not  recursive,  and  since  it  requires 
linear-equation  solving,  it  is  of  order  which  thus  yields  a  rather  high  numer¬ 
ical  complexity.  It  should  be  possible  to  produce  a  recursive  algorithm  for  the 
computation  of  0,  but  this  issue  will  not  be  pursued  here.  Moreover,  given  the 
parallel  structure  of  the  manipulator,  the  associated  recursive  algorithm  should 
be  parallelizable  with  multiple  processors. 

For  purposes  of  direct  dynamics,  on  the  other  hand,  we  want  to  solve  for  q  from 
Eq.  (12.30).  Moreover,  for  simulation  purposes,  we  need  to  derive  the  state- variable 
equations  of  the  system  at  hand.  This  can  be  readily  done  if  we  define  r  =  q,  the 
state-variable  model  thus  taking  on  the  form 

q  =  r  (12.32a) 

r  =  M“‘  [-N(q,  r)r  +  t  +  A0]  (12.32b) 

In  light  of  the  matrix  inversion  of  the  foregoing  model,  then,  the  complexity  of  the 
forward  dynamics  computations  is  also  of  order  n^. 

Example  12.4.1.  Derive  matrix  L/  of  Eq.  (12.17)  for  a  manipulator  having  six 
identical  legs  like  that  of  Fig.  12.2,  the  actuators  being  placed  at  the  fourth  joint. 


Solution:  We  attach  coordinate  frames  to  the  links  of  the  serial  chain  of  the  /th  leg 
following  the  Denavit-Hartenberg  notation,  while  noting  that  the  first  three  joints 
intersect  at  a  common  point,  and  hence,  ri  =  r2  =  r3 .  According  to  this  notation, 
we  recall,  vector  is  directed  from  the  origin  Of  of  the  i  th  frame  to  the  operation 
point  of  the  manipulator,  which  in  this  case,  is  Cm  -  The  Jacobian  matrix  of  the  /th 
leg  then  takes  the  form 


Cl  02  03  ^4  05  06 

01  X  ri  02  X  ri  03  X  ri  04  X  r4  05  x  rs  05  x  rs 


j 


the  subscript  J  of  the  array  in  the  right-hand  side  reminding  us  that  the  vectors 
inside  it  pertain  to  the  /th  leg.  Thus,  matrix  J j  maps  the  joint-rate  vector  of  the  /th 
leg,  0  /,  into  the  twist  iM  of  the  platform,  i.e.. 


Clearly,  the  joint-rate  vector  of  the  /th  leg  is  defined  as 
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^  J  =  \Pj\  0j2  0j3  dj4  dj5  dje\ 

Now,  note  that  except  for  0 /4,  all  joint-rates  of  this  leg  are  passive  and  thus  need  not 
appear  in  the  mathematical  model  of  the  whole  manipulator.  Hence,  we  should  aim 
at  eliminating  all  joint-rates  from  the  above  twist-rate  relation,  except  for  the  one 
associated  with  the  active  joint.  We  can  achieve  this  if  we  realize  that 

X  eji  +  eji  x  r/i  =  0,  i  =  1,2,3 


Further,  we  define  a  3  x  6  matrix  A/  as 

Ay  =  [Ryi  1] 

with  R ji  defined,  in  turn,  as  the  cross-product  matrix  of  r/i .  Now,  upon  multiplica¬ 
tion  of  J/  by  A/  from  the  left,  we  obtain  a  3  x  6  matrix  whose  first  three  columns 
vanish,  namely. 


Ayjy  =  [o  0  0  €4  X  (r4  -  Ti)  65  X  (rs  -  ri)  ee  x  (rs  -  ri)]^ 

and  hence,  if  we  multiply  both  sides  of  the  above  twist-shape  equation  by  A/  from 
the  left,  we  will  obtain  a  new  twist-shape  equation  that  is  free  of  the  first  three  joint 
rates.  Moreover,  this  equation  is  three-dimensional,  i.e., 

•  •  • 

[e4  X  (r4  -  ri)04  +  €5  x  (rs  -  ri)05  +  ee  x  (rs  -  r^^ely  =  x  ryi  +  cm 

where  the  subscript  J  attached  to  the  brackets  enclosing  the  whole  left-hand  side 
again  reminds  us  that  all  quantities  therein  are  to  be  understood  as  pertaining  to  the 
/th  leg.  For  example,  04  is  to  be  read  e 74.  Furthermore,  only  0j4  is  associated  with 
an  active  joint  and  denoted,  henceforth,  by  i.e., 

qj  ^  0j4  (12.33) 

It  is  noteworthy  that  the  foregoing  method  of  elimination  of  passive  joint  rates  is  not 
ad  hoc  at  all.  While  we  applied  it  here  to  the  elimination  of  the  three  joint  rates  of 
a  spherical  joint,  it  has  been  formalized  and  generalized  to  all  six  lower  kinematic 
pairs  (Angeles  1994). 

•  • 

We  have  now  to  eliminate  both  Ojs  and  Oje  from  the  foregoing  equation.  This 
can  be  readily  accomplished  if  we  dot-multiply  both  sides  of  the  same  equation  by 
vector  u  j  defined  as  the  cross  product  of  the  vector  coefficients  of  the  two  passive 
joint  rates,  i.e.. 


uy  =  [es  X  (rs  -  rO]/  x  [ee  x  (rj  -  rO]/ 
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We  thus  obtain  a  third  twist-shape  relation  that  is  scalar  and  free  of  passive  joint 
rates,  namely, 


Uj  ■  [64  X  (r4 


ri)^4]y  =  uy  •  x  r/i  +  cm) 


The  above  equation  is  clearly  of  the  form 


^j<ij=yjiM<  4j  =  (^4)j,  j  =  I,  II, 


with  and  y  j  defined,  in  turn,  as 


=  u/  •  e/4  X  (r/4  —  r/i) 


(12.34a) 


yj  = 


-r/i  X  uj 

u/ 


(12.34b) 


Upon  assembling  the  foregoing  six  scalar  twist-shape  relations,  we  obtain  a  six¬ 
dimensional  twist-shape  relation  between  the  active  joint  rates  of  the  manipulator 
and  the  twist  of  the  moving  platform,  namely. 


Zq  =  YIm 


with  the  obvious  definitions  for  the  two  6x6  matrices  Y  and  Z  given  below: 


Z  =  diag(^/,  ^//,  . . . ,  ^vi) 


We  now  can  determine  matrix  T  of  the  procedure  described  above,  as  long  as  Y  is 
invertible,  in  the  form 


T  =  Y“^Z 

whence  the  leg-matrix  L/  of  the  same  procedure  is  readily  determined,  namely, 

Ly  =  J7'T 

Therefore,  all  we  need  now  is  an  expression  for  the  inverse  of  the  leg  Jacobian  J/. 
This  Jacobian  is  clearly  full,  which  might  discourage  the  reader  from  attempting  its 
closed-form  inversion.  However,  a  closer  look  reveals  that  this  Jacobian  is  similar 
to  that  of  decoupled  manipulators,  studied  in  Sect.  5.2,  and  hence,  its  closed-form 
inversion  should  be  reducible  to  that  of  a  3  x  3  matrix.  Indeed,  if  we  recall  the 
twist-transfer  formula  of  Eqs.  (5.12a  and  b),  we  can  then  write  J/  as 


J/  ^  U/K/ 
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where  U/  is  a  unimodular  6x6  matrix  and  K/  is  the  Jacobian  of  the  same  /th  leg, 
but  now  defined  with  its  operation  point  located  at  the  center  of  the  spherical  joint. 
Thus, 


Kii  Ki2 

O  K22 


the  superscript  J  indicating  the  /th  leg  and  with  the  definitions  below: 

O:  the  3x3  zero  matrix; 

1:  the  3x3  identity  matrix; 

0/1 :  the  cross-product  matrix  of  O/i,  the  position  vector  of  the  center  of  the 

spherical  joint; 

Cm  •  the  cross  product  matrix  of  cm  ^  the  position  vector  of  Cm  • 

Furthermore,  the  3x3  blocks  of  K/  are  defined,  in  turn,  as 


(Kii)/  =  [d  62  63]^ 

(K12)/  =  [64  65  ee]^ 

(K22)y  =  [64  X  (r4  -  ri)  65  x  (rj  -  r/  ee  x  (rj  -  r/]^ 
Now,  if  the  inverse  of  a  block  matrix  is  recalled,  we  have 


o 


-Kn‘Ki2K 


-1 

22 


J 


where  the  superscript  of  the  blocks  has  been  transferred  to  the  whole  matrix,  in  order 
to  ease  the  notation.  The  problem  of  inverting  K /  has  now  been  reduced  to  that  of 
inverting  two  of  its  3  x  3  blocks.  These  can  be  inverted  explicitly  if  we  recall  the 
concept  of  reciprocal  bases  (Brand  1965).  Thus, 


1 


^11 


(62  X  63) 
(63  X  61) 
_(6i  X  62) 


T- 

T 

T 


1 

^22 


[(05  X  S5)  X  (06  X  S5)] 
[(06  X  S5)  X  (04  X  S4)] 
_[(04  X  S4)  X  (05  X  S5)] 


T- 

T 

T 


with  s /4,  s /5,  Aj^^,  and  A22  defined  as 


S/4  =  r/4  —  r/i 
S/5  =  r/5  —  T/i 

Afj  =  det(K5'/  =  (61  X  62  •  63)/ 

=  det(K4)  =  [(64  X  S4)  X  (6x85)  •  (66  X  Ss)]^ 
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the  subscripted  brackets  and  parentheses  still  reminding  us  that  all  vectors  involved 
pertain  to  the  /th  leg.  Moreover,  since  U/  is  unimodular,  its  inverse  is  simply 

1  O' 

Cm  —  0/1  1 


and  hence, 


Kr/K 


->K 


12K^2  (Ca^  -  Oyi)  -Kn 

KzMCm-Oji)  K 


12K 
-1 
22 


2^ 


the  matrix  sought,  L / ,  then  being  calculated  as 

Lj  =  J^Y-^Z 


While  we  have  a  closed- form  inverse  of  J/ ,  we  do  not  have  one  for  Y,  which  is  full 
and  does  not  bear  any  particular  structure  that  would  allow  us  its  inversion  explicitly. 
Therefore,  matrix  L /  should  be  calculated  numerically. 


12.5  Dynamics  of  Rolling  Robots 

The  dynamics  of  rolling  robots,  similar  to  that  of  other  robotic  mechanical  systems, 
comprises  two  main  problems,  inverse  and  direct  dynamics.  We  will  study  both 
using  the  same  mathematical  model.  Hence,  the  main  task  here  is  to  derive  this 
model.  It  turns  out  that  while  rolling  robots  usually  are  nonholonomic  mechanical 
systems,  their  mathematical  models  are  formally  identical  to  those  of  holonomic 
systems.  The  difference  between  holonomic  and  nonholonomic  systems  lies  in 
that,  in  the  former,  the  number  of  independent  actuators  equals  the  necessary  and 
sufficient  number  of  variables — independent  generalized  coordinates  in  Lagrangian 
mechanics — defining  a  posture  (configuration)  of  the  system.  In  nonholonomic 
systems,  however,  the  necessary  and  sufficient  number  of  variables  defining  a 
posture  of  the  system  exceeds  the  number  of  independent  actuators.  As  a  conse¬ 
quence,  in  holonomic  systems  the  dof  equals  the  number  of  independent  actuators. 
In  nonholonomic  systems,  the  dof  is  usually  defined  as  the  necessary  and  sufficient 
number  of  variables  defining  the  system  posture,  while  the  number  of  independent 
actuators  is  termed  the  system  mobility,  which  thus  turns  out  to  be  smaller  than  the 
system  dof.  Therefore,  relations  between  these  dependent  and  independent  variables 
will  be  needed  and  will  be  derived  in  the  course  of  our  discussion.  Moreover,  we  will 
study  robots  with  both  conventional  and  omnidirectional  wheels.  Of  the  latter,  we 
will  focus  on  robots  with  Mekanum  wheels. 
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Fig.  12.6  A  two-dof  rolling  robot:  (a)  its  general  layout;  and  (b)  a  detail  of  its  actuated  wheels 


12.5.1  Robots  with  Conventional  Wheels 

We  study  here  the  robot  of  Fig.  10.17,  under  the  assumption  that  it  is  driven  by 
motors  collocated  at  the  axes  of  its  two  coaxial  wheels,  indicated  as  Mi  and  M2  in 
Fig.  10.17b.  For  quick  reference,  we  repeat  this  figure  here  as  Fig.  12.6. 

Our  approach  will  be  one  of  multibody  dynamics;  for  this  reason,  we  distinguish 
five  rigid  bodies  composing  the  robotic  mechanical  system  at  hand.  These  are  the 
three  wheels  (two  actuated  and  one  caster  wheels),  the  bracket  carrying  the  caster 
wheel,  and  the  platform.  We  label  these  bodies  with  numbers  from  1  to  5,  in  the 
foregoing  order,  while  noticing  that  bodies  4  and  5,  the  bracket  and  the  platform, 
undergo  planar  motion,  and  hence,  deserve  special  treatment.  The  6x6  mass 
matrices  of  the  first  three  bodies  are  labeled  Mi  to  M3,  with  a  similar  labeling  for 
their  corresponding  six-dimensional  twists,  the  counterpart  items  for  bodies  4  and 
5  being  denoted  by  M4,  M^,  t^,  and  the  primes  indicating  3  x  3 — as  opposed 
to  6  X  6  in  the  general  case — mass  matrices  and  three-dimensional — as  opposed  to 
six-dimensional  in  the  general  case — twist  arrays. 

We  undertake  the  formulation  of  the  mathematical  model  of  the  mechanical 
system  under  study,  which  is  of  the  general  form  of  Eq.  (12.8a)  derived  for 
holonomic  systems.  The  nonholonomy  of  the  system  brings  about  special  features 
that  will  be  highlighted  in  the  derivations  below. 

As  a  first  step  in  our  formulation,  we  distinguish  between  actuated  and  unac¬ 
tuated  joint  variables,  grouped  into  vectors  Oa  and  0^,  respectively,  their  time- 

•  • 

derivatives  being  the  actuated  and  unactuated  joint  rates,  0  a  and  Ou,  respectively. 
From  the  kinematic  analysis  of  this  system  in  Sect.  10.5.1,  it  is  apparent  that  the 
foregoing  vectors  are  all  two-dimensional,  namely. 
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0 


a 


0i 

02 


(12.35) 


Further,  we  set  to  deriving  expressions  for  the  twists  of  the  five  moving  bodies  in 
terms  of  the  actuated  joint  rates,  i.e.,  we  write  those  twists  as  linear  transformations 
of  Oa,  i.e.. 


ti  =TiOa,  i  =  1,2,3  (12.36a) 

and 


t'i=T'0a,  i=4,5  (12.36b) 

where,  from  Eqs.  (10.41a  and  b),  (10.44a  and  b),  and  (10.48), 


T 


1 


— i  +  p8k  —p8k 

rj  0 


p8k  —  (i  +  p5k) 

0  rj 


03 

G3 


p8  —p8 

1 

_ 1 

_r(Ai  +  (l/2)j)  r(-Ai  +  (l/2)j)_ 

LcsJ 

(12.37) 

(12.38) 

(12.39) 

(12.40) 

(12.41) 


with  03,  G3,  0 4  and  G4  yet  to  be  derived.  In  the  sequel,  we  will  find  convenient 
to  work  with  a  few  nondimensional  parameters,  a,  8,  p — already  defined  in 
Eq.  (10.53) — and  A,  which  is  introduced  now,  and  displayed  below  with  the  first 
three  parameters  for  quick  reference: 


a  -\-  b 


a  = 


8  = 


d 

7’ 


(12.42) 


In  the  derivations  below,  we  resort  to  the  notation  introduced  in  Sect.  10.5. 1 .  First, 
we  note  that,  from  Eqs.  (10.45),  (10.52a  and  b),  we  can  write,  with  Otj  denoting  the 
(/,  j)  entry  of  0,  as  derived  in  Sect.  10.5.1, 


(^3  —  (^11 7  +  0l2^2)^3  +  [p8(0i  —  ^2)  +  ^21 7  +  022^2]k  (12.43) 


or 


0)3  =  OsOa 


(12.44) 
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with  03  defined  as 

®3  =  [01ie3  +  (021  +  pi5)k  01263  +  (022  -  pi5)k] 
In  more  compact  form, 


—  [^11^3  +  02lk  012^3  +  ^22^] 


with  O21  and  ^22  defined,  in  turn,  as 


^21  =  ^21  +  pS,  O22  =  ^22  ~  pS 


Moreover, 


C3  —  — r^3f3  —  —r(0ii0i  +  0l2^2)f3 


and  hence. 


G3  —  r  [—Onf^  — 0i2f3] 


(12.45a) 


(12.45b) 


(12.46) 


Further,  it  is  apparent  from  Fig.  12.6  that  the  scalar  angular  velocity  of  the  bracket, 
0)4,  is  given  by 


(JO4  =  CO 


xj/ 


and  hence. 


(O4  —  p8{di  —  62)  +  O21O1  +  022  ^2  —  02101  +  02202 


Therefore,  we  can  write 


co4  =  0l0a  (12.47a) 

where  ^  4  is  defined  as 

04  =  [021  022]^  (12.47b) 

Now,  since  we  are  given  the  inertial  properties  of  the  bracket  in  bracket  coordinates, 
it  makes  sense  to  express  64  in  those  coordinates,  taking  into  account  that  point  C4 
lies  in  the  middle  of  the  line  PO3.  Such  an  expression  is  obtained  below: 
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•  •  •  • 

Upon  expressing  0^  and  xj/  in  terms  of  0i  and  O2,  we  obtain 


^21^3  —  P^llf3 


di  d 


622^3  —  pOnh 


whence  it  is  apparent  that 

G4  =  d  [(1/2)021^3  —  P^llf3  (1/2)022^3  —  P^12f3] 


Therefore, 


_d[(l/2)02l^3  —  P^llf3]  <^[(1/2)022^3  —  P^12f3]_ 


(12.48) 


(12.49) 


(12.50) 


thereby  completing  all  needed  twist-shaping  matrices. 

The  2  X  2  matrix  of  generalized  inertia,  I(^),  is  now  obtained.  Here  we  have 
written  this  matrix  as  a  function  of  all  variables,  independent  and  dependent,  arrayed 
in  the  four-dimensional  vector  0 ,  because  we  cannot  obtain  an  expression  for  0  „ 
in  terms  of  0 a,  given  the  nonholonomy  of  the  system  at  hand.  Therefore,  I  is,  in 
general,  a  function  of  Oi,  O2,  O3,  and  To  be  sure,  from  the  above  expressions  for 
the  twist-shaping  matrices  T,  and  TJ ,  it  is  apparent  that  the  said  inertia  matrix  is  an 
explicit  function  of  xj/  only,  its  dependence  on  0i  and  O2  being  implicitly  given  via 
vectors  03  and  f3.  We  derive  the  expression  sought  for  I  starting  from  the  kinetic 
energy,  namely. 


1  4 

or 

^  ^ETrM,T,j  0^  +  Ul 

and  hence. 


(t;)^m;t;  1 0, 


I  I 


(12.51) 


1  4 


(12.52) 


In  order  to  expand  the  foregoing  expression,  we  let  and  be  the  3x3  inertia 
matrices  of  the  two  actuated  wheels  and  the  caster  wheel,  respectively,  the  scalar 
moments  of  inertia  of  the  bracket  and  the  platform,  which  undergo  planar  motion, 
being  denoted  by  and  Ip.  Likewise,  we  let  mb,mc,  and  nip  denote  the  masses 
of  the  corresponding  bodies.  Therefore, 
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M 


1 


Jw  o 

O  m„lj. 


ic  o 

o  nich 


'h  0^  ■ 

0  mf,l2 


'Ip  0^  ■ 
_0  mp\2_ 


with  O  and  I3  denoting  the  3x3  zero  and  identity  matrices,  while  0  and  I2  the 
two-dimensional  zero  vector  and  the  2x2  identity  matrix.  Furthermore,  under  the 
assumption  that  the  actuated  wheels  are  dynamically  balanced,  we  have 


7  0  0 
0  77  0 
0  0  77 


Moreover,  we  assume  that  the  caster  wheel  can  be  modeled  as  a  rigid  disk  of  uniform 
material  of  the  given  mass  nic  and  radius  r,  and  hence,  in  bracket-fixed  coordinates 
{es,  f3,  k}. 


2 

-rrirf 

4 


200 
0  1  0 
00  1 


It  is  now  a  simple  matter  to  calculate 

I  +  (pS^H  +  —(pS^H 

-{p8fH  (p8)^H  _ 

(p8fH  -{p8fH 
—(p8)^H  I  +  {p8yH  +  m^r^_ 

where  the  symmetry  between  the  two  foregoing  expressions  is  to  be  highlighted:  that 
is,  the  second  expression  is  derived  if  the  diagonal  entries  of  the  first  expression  are 
exchanged,  which  is  physically  plausible,  because  such  an  exchange  is  equivalent 
to  a  relabeling  of  the  two  wheels.  The  calculation  of  the  remaining  products  is  less 
straightforward  but  can  be  readily  obtained.  From  the  expressions  for  T3  and  M3, 
we  have 


T[MiTi  = 

T[M2T2  = 


0 

©3 

_0  mch_ 

.G3. 

=  [©3^  G[] 


=  ©^Jc03  +  G3 
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In  order  to  calculate  the  foregoing  products,  we  write  and  03  in  component 
form,  i.e.. 


^  1  7 

Jc©3  =  -mcr 

"2  0  0" 

■011 

012 

1  2 

=  -ntcr 

A  ^ 

2011 

2012 

0  1  0 

0 

0 

0 

0 

4 

_0  0  1_ 

_021 

022_ 

4 

_  021 

022  _ 

and  hence. 


2 

-rricr 

4 


20^1  +  O21  2O11O12  +  O21O22 

2O11O12  +  O21O22  ^^12  ^22 


Likewise, 


m3G^G3  =  rucr 


oh  O11O12 


11 


011^12  0 


2 

12 


Further, 


(t;)^m;t;  =  [^4  g[] 


■4  0^  ■ 

1 - 

1 _ 

_0  mb\2_ 

LG4J 

—  1 1)0  4$^  +  m^G4  G4 


T, 


Upon  expansion,  we  have 


O21  O21O22 


O21O22  0 


1 


+  -nibd^ 


22 

—2 


O21  +  ^P^O^^  O21O22  +  ^P^OiiOu 

O21O22  +  4p^0ii0i2  O22  +  4p^0j 


|2 

12 


Finally, 


(T')^M'T'  =  [^5 


1 

0 

_ 1 

-0l 

1 

0 

K) 

1 _ 

Gs. 

—  IpOsOh  -\-mpGTG5 


p 


P^5 


which  can  be  readily  expanded  as 


(T')^M'T'  =  Ip(p8f 


■  1  -f 

,  2 

-1  1  _ 

+  nipr 

■(1/4) +  A2  (1/4) 


We  can  thus  express  the  generalized  inertia  matrix  as 

I  =  Iw  +  Ic  +  +  I;? 
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where  1^;,  Ic,  I^,  and  Ip  denote  the  contributions  of  the  actuated  wheels,  the  caster 
wheel,  the  bracket,  and  the  platform,  respectively,  i.e.. 


7  +  2{p8fH  +  m„r^  -2ip8fH 

-2(pSfH  1  +  2{p8fH  +  m„r^ 


Ir  = 


nicr^ 


21 


6^11^12  +  ^21^22 
—2 


6^11^12  +  ^21^22  6^12  +  ^ 


22 


—  Ib 


^21  ^21^22 


^21^22  ^ 


1 


+  -nibd^ 


22 


^21  + 


lai 
11 

2, 


^21^22  +  4p^0ii0i2 
21^22  +  4p^ 011^12  ^22 


Ip  =  Ip(p8) 


■  1  -1 

,  2 

-1  1  _ 

+  tripr 

(1/4) +  A2  (1/4) -A^' 


It  is  now  apparent  that  the  contributions  of  the  actuated  wheels  and  the  platform 
are  constant,  while  those  of  the  caster  wheel  and  the  bracket  are  configuration- 
dependent.  Therefore,  only  the  latter  contribute  to  the  Coriolis  and  centrifugal 
generalized  forces.  We  thus  have 


T^Mt  =  T[M3t3  +  (T;)^M;t; 

From  the  expression  for  T^M3T3,  we  obtain 


T^Matj  =  0[J,03  +W3C[C3 


the  time-derivatives  being  displayed  below: 

=  [^11^3  +  0iiCO4f3  +  02lk  ^12^3  +  6120)4^  +  ^22 k] 
C3  =  r  [— 0iif3  +  ^110)403  — 0l2f3  +  ^12<^4C3] 

with  the  time-derivatives  of  the  entries  of  0  given  as 


Q  =  ^j/ 


—a  sin +  (cos  V^)/2  o'  sin  i/^  +  (cos  V^)/2 
p[— O'  cos  i/a  —  (sin  i/a) /2]  p[Q'  cos  i/a  —  (sin  i/a) / 2] 


(12.53) 


its  parameters  being  defined  in  Eq.  (12.42).  Upon  expansion,  the  products  appearing 

rri  * 

in  the  expression  for  T3  M3  J  3  become 
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Therefore, 


=  — r- 


2  r 


m3CrC3  =  fUcr 


2^11^11  +  ^21^21  2^11^12  +  ^21^22 
2^12^11  +  ^22^21  2^12^12  +  ^22^22. 

^11^11  ^11^12 
^12^11  ^12^12 


rp  .  m^r 

T3^M3T3  =  ^ 


2  r 


6^11^11  +  ^21^21  6^11^12  +  O21O22 
6^12^11  +  ^22^21  6^12^12  +  O22O22 


Likewise, 


(T;)^M;t;  =  11,0^01  + 


the  above  time-derivatives  being 


^4  —  [^21  ^22] 

C4  =  d  [(31103  +  <^12f3  <^21^3  +  (322f3] 


with  coefficients  Cfj  given  below: 


1  . 


l-r 


Cii  —  -O21  +  pOii(j)/[  ,  C12  —  -^21<^4  —  P^ll 


1  , 


1^ 


<^21  —  -^22  +  p0l2O)4  ,  C22  —  -022(^4  “  P^12 


Hence, 


i^e^ol  =  I. 


1 


^21^21  ^21^22 
O22O21  O22O22 


mbClC4  =  -ruhd^ 


02\Cn  —'^pOnCu  02\C2i  —  2p0iiC22 
L^22<2ii  —  2p012Cl2  ^22<221  “  2p012C22. 


Therefore, 


(T;)^M;t;  =  i 


OilOll  &2\S22 

O22O2I  O22O22 


1 


+  -rubd^ 


02iCn  —IpOiiCu  O21C21  —2p0iiC22 
L^22<^11  —  2p012Cl2  ^22<^21  “  2p012C22. 


In  the  final  steps,  we  calculate  WMT.  As  we  saw  earlier,  only  the  caster  wheel 
and  the  bracket  can  contribute  to  this  term,  for  the  contributions  of  the  other  bodies 
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to  the  matrix  of  generalized  inertia  are  constant.  However,  the  bracket  undergoes 
planar  motion,  and  according  to  Exercise  7.10,  its  contribution  to  this  term  vanishes. 
Therefore, 


T^WMT  =  T^WsMsTs 

Upon  expansion  of  the  foregoing  product,  we  have 

03 

G3. 

=  0[a3lc03  (12.54) 

The  foregoing  term  vanishes,  as  we  prove  below.  First,  notice  that 


=  [03^  Gj^] 


^3  0 

1 - 

0 

1 _ 

0  0 

0  nich 

^^30)3  —  0 


However,  from  Eq.  (12.44), 


a 


and  hence, 

for  every  0 a,  whence 

^3^3  =  O32 


with  O32  denoting  the  3x2  zero  matrix.  Upon  transposing  the  foregoing  expression, 
we  obtain 


3  —  O23 

where  we  have  recalled  that  ^^3  is  skew- symmetric. 

Substitution  of  the  above  expression  into  Eq.  (12.54)  readily  shows  that  the  term 
in  question  indeed  vanishes,  i.e.. 

In  summary,  the  Coriolis  and  centrifugal  force  terms  of  the  system  at  hand  take 
the  form 


4 


6^11(^11^1  +  ^12^2)  +  ^21(^12^1  +  ^22^2) 
6^12(^11^1  +  ^12^2)  +  ^22(^12^1  +  ^22^2) 


+  ^^(^21^1  +  ^22^2) 


^21 

^22 


+  +  <^21^2) 


^21 

O22 


-rubd^picnOi  +  C22O2) 


On 

0\2 


C(0,0a)0a 
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If  we  recall  that  the  Ctj  coefficients  are  linear  in  the  joint  rates,  then  the  foregoing 
expression  clearly  shows  the  quadratic  nature  of  the  Coriolis  and  centrifugal  terms 
with  respect  to  the  joint  rates. 

The  derivation  of  the  forces  supplied  by  the  actuators  is  straightforward: 


T  = 


-Cl 

ri 


The  dissipative  generalized  force  is  less  straightforward,  but  its  calculation  is  not 
too  lengthy.  In  fact,  if  we  assume  linear  dashpots  at  all  joints,  then  the  dissipation 
function  is 


-Oa  CuOa  +  -Oj,  C34OU 


with  C12  and  C34  defined  as 


C 


12 


Cl  0 

0  C2 


C3  0 
0  C4 


Now,  if  we  recall  the  expression  for  in  terms  of  ^ a,  we  end  up  with 


a 


D  being  defined,  in  turn,  as  the  equivalent  damping  matrix,  given  by 


D  =  C12  +  0^C340 


Since  0  =  0(V^),  D  =  D(V^),  the  dynamics  model  under  study  thus  taking  the 
form 


with  I  and  C(0 ,  Oa)  given,  such  as  in  the  case  of  holonomic  systems,  as 

I(^)  =  T^MT 

C(e  Ja)  =  T^MT  +  T^WMT 

thereby  completing  the  mathematical  model  governing  the  motion  of  the  system 
at  hand.  Note  here  that  0  denotes  the  four-dimensional  vector  of  joint  variables 
containing  all  four  angles  appearing  as  components  of  and  Ou-  Because  of  the 
nonholonomy  of  the  system,  an  expression  for  the  latter  in  terms  of  the  former 
cannot  be  derived,  and  thus  the  whole  four-dimensional  vector  0  is  left  as  an 
argument  of  both  I  and  C. 
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Fig.  12.7  Rolling  robot  with  ODWs  in  a  A-array 


Note  that  calculating  the  torque  t  required  for  a  given  motion — inverse 
dynamics — of  the  rolling  robot  under  study  is  straightforward  from  the  above 
model.  However,  given  the  strong  coupling  among  all  variables  involved,  a 
recursive  algorithm  in  this  case  is  not  apparent.  On  the  other  hand,  the  determination 
of  the  motion  produced  by  a  given  history  of  joint  torques  requires  (a)  the  calculation 
of  I,  which  can  be  achieved  symbolically;  (b)  the  inversion  of  I,  which  can  be  done 
symbolically  because  this  is  a  2  x  2  matrix;  (c)  the  calculation  of  the  Coriolis  and 
centrifugal  terms,  as  well  as  the  dissipative  forces;  and  (d)  the  integration  of  the 
initial-value  problem  resulting  once  initial  values  to  0  and  0  a  have  been  assigned. 


12.5.2  Robots  with  Omnidirectional  Wheels 


We  now  consider  a  three-dof  robot  with  three  actuated  wheels  of  the  Mekanum  type, 
as  shown  in  Fig.  10.19,  with  the  configuration  of  Fig.  12.7,  which  will  be  termed, 
henceforth,  the  A-array.  This  system  is  illustrated  in  Fig.  12.8. 

Below  we  will  adopt  the  notation  of  Sect.  10.5.2,  with  ot  =  n j 2  and  n  =  3. 
We  now  recall  that  the  twist  of  the  platform  was  represented  in  planar  form  as 


(12.55) 


where  co  is  the  scalar  angular  velocity  of  the  platform  and  c  is  the  two-dimensional 
position  vector  of  its  mass  center,  which  will  be  assumed  to  coincide  with  the 


12.5  Dynamics  of  Rolling  Robots 


535 


Fig.  12.8  A  view  of  the 
three- wheeled  robot  with 
Mekanum  wheels  in  a 
A -array 


centroid  of  the  set  of  points  {  Q  Moreover,  the  three  wheels  are  actuated,  and 
hence,  the  three-dimensional  vector  of  actuated  joint  rates  is  defined  as 


~er 

Oi 

A. 


(12.56) 


The  relation  between  0 a  and  t'  was  derived  in  general  in  Sect.  10.5.2.  As  pertaining 
to  the  robot  of  Fig.  12.7,  we  have 


JK  =  Kt' 

with  the  two  3x3  Jacobians  J  and  K  defined  as 


frn 

^3 


(12.57a) 


(12.57b) 


where,  it  is  recalled,  a  is  the  height  of  the  axis  of  the  wheel  hub  and  r  is  the 
horizontal  distance  of  the  points  of  contact  with  the  ground  to  the  mass  center  C  of 
the  platform,  as  indicated  in  Fig.  12.7a.  Moreover,  vectors  {  and  {  f,  }j,  defined 
in  Sect.  10.5.2,  are  displayed  in  Fig.  12.7.  Below  we  derive  expressions  for  co  and 
c,  from  Eq.  (12.57a),  in  terms  of  the  joint  rates.  To  this  end,  we  expand  these  three 
equations,  thus  obtaining 


r CO  f^c  =  —aOi  (12.58a) 

rco  f 2^  =  —a02  (12.58b) 

rco  +  f^c  =  —aO^  (12.58c) 
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Upon  adding  corresponding  sides  of  the  three  foregoing  equations,  we  obtain 

3  3 

Srco  +  ft  — 

1  1 

But  from  Fig.  12.7b,  it  is  apparent  that 

01  +  02  +  03  =  0 

fl  +  f2  +  f3  =0 

Likewise, 


V3 

Cl  —  ^  (f3  f2),  C2  — 

^  (fl  f3),  C3  — 

3  (ft-r,) 

(12.60c) 

fl  —  ^  (C2  C3),  f2  — 

^  (63  ei),  f3  - 

3  (ei  62) 

(12.60d) 

and  hence,  the  above  equation  for  co  and  c  leads  to 


(12.59) 


(12.60a) 

(12.60b) 


a 


CO  =  — 


(12.61) 


Now  we  derive  an  expression  for  c  in  terms  of  the  actuated  joint  rates.  We  do 
this  by  subtracting,  sidewise,  Eq.  (12.58b)  from  Eq.  (12.58a)  and  Eq.  (12.58c)  from 
Eq.  (12.58b),  thus  obtaining  a  system  of  two  linear  equations  in  two  unknowns,  the 
two  components  of  the  two-dimensional  vector  c,  namely. 


with  matrix  A  and  vector  b  defined  as 

0i  —  O2 
O2  —  ^3_ 

where  we  have  used  relations  (12.60c).  Since  A  is  a  2  x  2  matrix,  its  inverse  can  be 
readily  found  with  the  aid  of  Facts  5.7.3  and  5.7.4,  which  yield 

C  =  -(3  [-E01  E03J 


61  —  62 
O2  —  O3 


A  = 


(f2-f3)^ 


--V3 


T 
T 

J 


h  =  —a 


Now,  from  Fig.  12.7b, 


E01  —  fl,  E03  —  f3 
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and  hence,  c  reduces  to 

C  =  ^^[(^2  —  ^l)fl  +  (O2  —  ^3)f3]  =  “^[^2  (ft  +  f3)  —  —  ^3f3] 

But  by  virtue  of  Eq.  (12.60b), 


ft  +  f3  —  — f2 


the  above  expression  for  c  thus  becoming 

2a  • 

c  =  (12.62) 

1 

Thus,  CO  is  proportional  to  the  mean  value  of  {  ft  }^,  while  c  is  proportional  to  the 
mean  value  of  {  ft  f/  } ^ .  In  deriving  the  mathematical  model  of  the  robot  at  hand, 
we  will  resort  to  the  natural  orthogonal  complement,  and  therefore,  we  will  require 
expressions  for  the  twists  of  all  bodies  involved  in  terms  of  the  actuated  wheel  rates. 
We  start  by  labeling  the  wheels  as  bodies  1,  2,  and  3,  with  the  platform  being  body  4. 
Moreover,  we  will  neglect  the  inertia  of  the  rollers,  and  so  no  labels  need  be  attached 
to  these.  Furthermore,  the  wheel  hubs  undergo  rotations  with  angular  velocities  in 
two  orthogonal  directions,  and  hence,  a  full  six-dimensional  twist  representation 
of  these  will  be  required.  Henceforth,  we  will  regard  the  angular  velocity  of  the 
platform  and  the  velocity  of  its  mass  center  as  three-dimensional  vectors.  Therefore, 


t4  =  T4^ 


a  ? 


k  k  k 

2rfi  2rf2  2rf3 


with  A  defined,  in  turn,  as  the  ratio 


Now,  the  wheel  angular  velocities  are  given  simply  as 


(O 


=  diet  -\-cok  =  diet  -  A  j  ^  ft  j  k 


(12.63) 


(12.64) 


(12.65) 


or 


0)1  =  (ei  —  Ak)ft  —  X02k  —  XO^k 
0)2  =  — A^ik  +  (02  —  Ak)02  —  XO^k 

0)3  =  — A^ik  —  A^2k  +  (03  —  Ak)03 


(12.66a) 

(12.66b) 

(12.66c) 
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Similar  expressions  are  derived  for  vectors  C/ .  To  this  end,  we  resort  to  the  geometry 
of  Fig.  12.7,  from  which  we  derive  the  relations 

c,  =  c  +  corfi  =  -2Xr  j  -  Xr  j  f, 

and  hence. 

Cl  =  — Ar[(30i  -\-  62  -\-  ^3)fi  +  2(^2f2  H”  ^sfs)]  (12.67a) 

C2  =  -Arp^ifi  +  (^1  +  34  +  ^3)f2  +  203f3]  (12.67b) 

C3  =  -Ar[2(0ifi  +  4f2)  +  (^1  +  4  +  3^3)f3]  (12.67c) 


From  the  foregoing  relations,  and  those  for  the  angular  velocities  of  the  wheels, 
Eqs.  (12.66a-c),  we  can  now  write  the  twists  of  the  wheels  in  the  form 

ii  i  =  1,2,3  (12.68) 


where 


Ti 

T2 

T3 


01  —  Ak  — Ak  — Ak 

— 3Arfi  — Ar(fi  +  2f2)  — Ar(fi  +  2f3) 

— Ak  02  —  Ak  — Ak 

-Ar(f2  +  2fi)  -3Arf2  -Ar(f2  +  2f3) 

— Ak  — Ak  03  —  Ak 

— Ar(f3  +  2fi)  — Ar(f3  +  2f2)  — 3Arf3 


On  the  other  hand,  similar  to  what  we  have  in  Eq.  (12.62),  an  interesting  relationship 
among  angular  velocities  of  the  wheels  arises  here.  Indeed,  upon  adding  the 
corresponding  sides  of  the  three  equations  (12.66a-c),  we  obtain 

33  3 

^  ft);  =  ^  fte,-  -  3Ak  ^  9i 

1  1  1 


Further,  we  dot-multiply  the  two  sides  of  the  foregoing  equation  by  k,  which  yields, 
upon  rearrangement  of  terms. 


3 

3A 

1 

and  by  virtue  of  Eq.  (12.61), 


u)  =  k  •  0), 


(12.69) 


that  is,  the  vertical  component  of  the  mean  wheel  angular  velocity  equals  the  scalar 
angular  velocity  of  the  platform. 
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Now  we  proceed  to  establish  the  mathematical  model  governing  the  dynamics  of 
the  system  under  study.  The  generalized  inertia  matrix  is  then  calculated  as 

4 

I  =  ^TfM,T,-  (12.70) 

1 

where,  if  1^  and  denote  the  moment-of-inertia  matrix,  in  body-hxed  coordinates, 
and  the  mass  of  each  of  the  three  wheels,  with  similar  dehnitions  for  Ip  and  rrip  SiS 
pertaining  to  the  platform. 


Iw  o 

O 


i  =  1,2,3,  M4 


Ip  O 

O  nipl 


(12.71) 


We  will  also  need  the  angular- velocity  dyads,  W, ,  which  are  calculated  as 


(12.72) 


where  W4  will  not  be  needed,  since  the  platform  undergoes  planar  motion.  We  have 

jyj  rp  _  Ak)  Al^k  Al^k 

^  ^  _  -Sm^Arfi  -mw;Ar(fi  +  2f2)  +  2f3)_ 

Moreover,  we  assume  that  in  a  local  coordinate  frame  {  e, ,  f/ ,  k  }, 


7  0  0 
0/0 
0  0/ 


in  which  I  and  /  are  constants.  Hence, 


T[MiTi 


■/  +  A^^  AV  AV 
AV  a^l  a^m 
AV  a^m  a^l 


K  =  J  9m^r^ 
L  =  J  -\-  3m^r^ 
M  =  J  —  3m^r^ 


where 
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Likewise, 


T[M2T2 


~X^L  X^J  X^M~ 
AV  I  +X'^K  AV 
A^M  A^A  A^L 


T[M3T3 


"A^L  X^J  AV 
AV  A^L  AV 

aV  aV  /  +  a^a: 


Furthermore, 


1V14T4  —  — A 


I;.k 


Ij^k  Ij^k 


Iniprfi  2m^rf2  Irriprf^ 


It  is  apparent  that,  by  virtue  of  the  planar  motion  undergone  by  the  platform,  only  its 
moment  of  inertia  H  about  the  vertical  passing  through  its  mass  center  is  needed. 
Then, 


T[M4T4  = 


H  +  AnipT^  H  —  IrUpV^  H  —  IrUpV^ 

H  —  InipT^  H  +  Arripr^  H  —  IrripT^ 

H  —  IrripT^  H  —  IrripT^  H  +  Arripr^ 


Upon  summing  all  four  products  computed  above,  we  obtain 


a  P  P 
P  a  P 
_P  P  a_ 


with  the  definitions  below: 


a  =  I  -\-  +  3/  +  15mH;r^  +  ArUpV^) 

P  =  [H  +  3/  —  —  Irripr^) 

which  is  a  constant  matrix.  Moreover,  note  that  the  geometric  and  inertial  symmetry 
assumed  at  the  outset  is  apparent  in  the  form  of  the  foregoing  inertia  matrix,  its 
inverse  being  readily  obtained  in  closed  form,  namely. 


a  P  —P  —P 
—P  a  P  —P 
—P  —P  a  P 


A  =  (o'  +  P)a  —  ip^ 


Next,  we  turn  to  the  calculation  of  the  T^MT  term.  This  is  readily  found  to  be 


4 

T^Mt  = 

1 
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each  of  the  foregoing  products  being  expanded  below.  We  have,  first, 


coil  0  0 

3Aru)ei  —Xrcoiid^,  —  ^2)  Xrcoiid^  —  62) 

0  a)f2  0 

Xrco(ei  —  63)  3Xr(joe2  — Ara)(ei  —  03) 

0  0  cof^ 

—Xr(jL>{id2  —  Cl)  Xr(jL>{id2  —  Cl)  SXrcoe^ 


0  0  0 

Ircoei  2r(joe2  Xrcoe^ 


Hence,  for  the  first  wheel. 


Miti 


0  0 

3Xm^r(jL>G;i  —Xm^r(jL>{id2  —  02)  Xm^rcoiid^  —  02) 


Therefore, 


T[Miti 


3\/3A' 


m 


w 


r^co 


0  -1  1 

1  0  0 

-10  0 


where  the  skew- symmetric  matrix  is  the  cross  product  matrix  of  vector  [0,  1,  1  ]^. 
By  symmetry,  the  other  two  products,  Tj M/  T/ ,  for  /  =  1 , 2,  take  on  similar  forms, 
with  the  skew-symmetric  matrix,  becoming,  correspondingly,  the  cross-product 
matrix  of  vectors  [1,  0,  1  and  [1,  1,  0]^.  This  means  that  the  first  of  these  three 
products  is  affected  by  the  rotation  of  the  second  and  the  third  wheels,  but  not  by 
that  of  the  first  one;  the  second  of  those  products  is  affected  by  the  rotation  of  the 
first  and  the  third  wheels,  but  not  by  the  second;  the  third  product  is  affected,  in  turn, 
by  the  rotation  of  the  first  two  wheels,  but  not  by  that  of  the  third  wheel.  We  thus 
have 


0  - 

-1 

0 

TIM2±2  = 

=  3\/3X^m^r^(jL> 

1 

0 

-1 

_0 

1 

0 

"  0 

0 

1 

=  ^V^X^niwr^co 

0 

0 

-1 

-1 

1 

0 

Furthermore, 


1V14T4  —  X 


0 

—Imprcoei 


0  0 

—2mpr(joe2  —2mpr(joe3 
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and  hence, 


T[M4t4 


0  -1  1 
1  0  -1 
-1  1  0 


(12.74a) 


whose  skew- symmetric  matrix  is  readily  identified  as  the  cross-product  matrix  of 
vector  [1,  1,  1  ]^,  thereby  indicating  an  equal  participation  of  all  three  wheels  in 
this  term,  a  rather  plausible  result.  Upon  adding  all  four  products  calculated  above, 
we  obtain 


T^MT  =  2\/3X^(3myv  +  mp)r^(jo 


0  -1  1 
1  0  -1 
-1  1  0 


(12.75) 


The  equal  participation  of  all  three  wheels  in  the  foregoing  product  is  apparent. 
Moreover,  notice  that  the  term  in  parentheses  can  be  regarded  as  an  equivalent  mass, 
which  is  merely  the  sum  of  all  four  masses  involved,  the  moments  of  inertia  of  the 
wheels  playing  no  role  in  this  term. 

We  now  turn  to  the  calculation  of  the  WMT  term,  which  can  be  expressed  as 
a  sum,  namely. 


3 

T^WMT  = 

1 

where  we  have  not  considered  the  contribution  of  the  platform,  because  this 
undergoes  planar  motion.  Moreover,  matrices  W/,  for  i  =  1,  2,  and  3,  take  the 
obvious  forms 


O 

o  o 


We  then  have,  for  the  first  wheel. 


WiMiTi  = 


(Oi  X  [lH;(ei  -  Ak)]  -coi  X  (AlH;k)  -coi  x  (AlH;k) 

0  0  0 


Now,  it  does  not  require  too  much  effort  to  calculate  the  complete  first  product, 
which  merely  vanishes,  i.e.. 


T[WiMiTi  =  O33 

with  O33  defined  as  the  3x3  zero  matrix.  By  symmetry,  the  remaining  two  products 
also  vanish,  and  hence,  the  sum  also  does,  i.e., 

T^WMT  =  O33 


(12.76) 
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Now,  calculating  the  dissipative  and  active  generalized  forces  is  straightforward. 
We  will  neglect  here  the  dissipation  of  energy  occurring  at  the  bearings  of  the 
rollers,  and  hence,  if  we  assume  that  the  lubricant  of  the  wheel  hubs  produces  linear 
dissipative  torques,  then  we  have 


r^ii 

• 

~ri~ 

5  =  c 

02 

,  r  = 

-Cl 

1 

_ 1 

(12.77) 


where  c  is  the  common  damping  coefficient  for  all  three  wheel  hubs.  We  now  have 
all  the  elements  needed  to  set  up  the  mathematical  model  governing  the  dynamics 
of  the  robot,  namely, 

iea  +  C{o))K  =  r-S  (12.78) 

where  C((jo)  =  T^MT  +  T^WMT;  from  Eqs.  (12.75)  and  (12.76),  this  term 
becomes 


C(ft))  =  2V3X^(3m„  +  mp)r^(jo 


0  -1  1 
1  0  -1 
-1  1  0 


(12.79) 


•  •  • 

Since  co  =  —a/i?>r){0i  +  ^2  +  ^3),  the  quadratic  nature  of  the  second  term 
of  Eq.  (12.78)  in  the  joint  rates  becomes  apparent.  It  is  also  apparent  that  the 
mathematical  model  derived  above  does  not  depend  on  0 a.  What  this  means  is  that 
the  mathematical  model  allows  the  integration  of  the  actuated  joint  accelerations  to 
yield  joint-rate  histories  0  ait),  but  this  model  cannot  provide  joint- variable  histories 
0  ait).  To  obtain  these,  for  given  initial  conditions,  the  joint-rate  histories  have  to  be 
integrated,  which  can  be  done  by  numerical  quadrature. 

Einally,  in  order  to  obtain  the  Cartesian  histories  of  the  platform  pose,  given 
by  the  angle  cr  that  a  specific  line  of  the  platform  makes  with  a  line  fixed  in  an 
inertial  frame,  and  the  position  vector  of  the  mass  center,  c,  Eqs.  (12.61)  and  (12.62) 
have  to  be  integrated.  While  the  integration  of  the  former  can  be  readily  done  by 
quadrature,  that  of  the  latter  requires  knowledge  of  vectors  f/,  for  /  =  1, 2,  3,  and 
these  vectors  depend  on  a.  Thus,  the  integration  of  Eq.  (12.61)  can  be  done  once 
the  joint-rate  histories  are  known;  that  of  Eq.  (12.62)  requires  knowledge  of  angle 
O'.  These  features  are  inherent  to  nonholonomic  systems. 


12.6  Exercises 

12.1  Show  that  the  mathematical  model  of  an  arbitrary  robotic  mechanical  system, 
whether  holonomic  or  nonholonomic,  with  r  rigid  bodies  and  n  degrees  of 
freedom,  can  be  cast  in  the  general  form 


+  C(^,  0 a)0 a  —  -\-  y  ^ 
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where 


0 

r 


y 

8 

m 

CiOJa) 


the  m -dimensional  vector  of  variables  associated  with  all  joints, 
actuated  and  unactuated; 

the  /I -dimensional  vector  of  actuated  joint  variables,  n  <  m; 

the  /I -dimensional  vector  of  actuator  torques; 

the  /I -dimensional  vector  of  gravity  torques; 

the  /I -dimensional  vector  of  dissipative  torques; 

the  n  X  n  matrix  of  generalized  inertia; 

the  n  xn  matrix  of  Coriolis  and  centrifugal  forces; 


with  I(^)  and  C(0 ,0a)  given  by 


I(^)  =  T^MT 

C(0Ja)  =  i[i  +  T^Mt 


t^MT  +  T^(WM  +  MW)T] 


in  which 

M:  the  6r  x  6r  matrix  of  system  mass; 

T:  the  n  x  6r  twist- shaping  matrix  that  maps  the  -dimensional  vector  of 
actuated  joint  rates  into  the  6 r -dimensional  vector  of  system  twist  t; 

W:  the  6r  X  6r  matrix  of  system  angular  velocity. 

•  • 

12.2  For  the  system  of  Exercise  12.1,  show  that  the  matrix  difference  1(0 , 0 a)  — 
2C(0,0a)  is  skew- symmetric.  This  is  a  well-known  result  for  holonomic 
systems  (Spong  et  al.  2006). 

12.3  For  the  rolling  robot  with  conventional  wheels  of  Sect.  12.5.1,  find  the 
generalized  inertia  matrix  of  the  robot  under  the  maneuvers  described 
below: 

(a)  pure  translation; 

(b)  midpoint  of  segment  Oi  O2  stationary. 

In  each  case,  give  a  physical  interpretation  of  the  matrix  thus  obtained. 

12.4  With  reference  to  the  same  robot  of  Exercise  12.3,  state  the  conditions  on  its 
geometric  parameters  that  yield  1^  and  Ip  isotropic,  these  two  2x2  matrices 
having  been  defined  in  Sect.  12.5.1. 

12.5  Derive  the  mathematical  model  governing  the  motion  of  a  two-dof  rolling 
robot  with  conventional  wheels,  similar  to  that  of  Fig.  10.17,  but  with  two 
caster  wheels  instead.  The  vertical  axes  of  the  caster  wheels  are  a  distance  / 
apart  and  a  distance  a -\-b  from  the  common  axis  of  the  driving  wheels.  What 
is  the  characteristic  length  of  this  robot? 

12.6  Find  the  conditions  under  which  the  three- wheeled  robot  with  omnidirec¬ 
tional  wheels  analyzed  in  Sect.  12.5.2  has  an  isotropic  inertia  matrix.  Discuss 
the  advantages  of  such  an  inertially  isotropic  robot. 
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12.7  With  reference  to  the  omnidirectional  robot  of  Sect.  12.5.2,  show  that  the 
mathematical  model  can  be  manipulated  to  yield  a  single  first-order  ordinary 
differential  equation  in  u),  of  the  form 

d)  +  kco  =  f  (t) 

in  which  k  is  a.  constant  with  units  of  frequency,  its  inverse  being  the  time- 
constant  of  the  system.  Find  expressions  for  k  and  / {t).  Then,  integrate  the 
above  equation  in  closed  form,  to  obtain  the  time-history  of  o)  for  a  given 
time-history  f(t)  and  given  initial  condition  u)(0). 

12.8  Establish  the  conditions  on  the  actuated  joint  rates  under  which  the  three- 
wheeled  robot  with  omnidirectional  wheels  of  Sect.  12.5.2  undergoes  pure 
translation.  Under  these  conditions,  the  robot  has  only  two  degrees  of 
freedom  and,  hence,  a  2  x  2  inertia  matrix.  Derive  an  expression  for  its  inertia 
matrix.  Hint:  The  constraint  for  pure  translation  can  be  written  as 

and  hence,  if  the  3x2  matrix  L  is  an  orthogonal  complement  of  a,  i.e.,  if 
a^L  =  O2 ,  where  O2  is  the  two-dimensional  zero  vector,  then  the  underlying 
Euler-Lagrange  equations  of  the  constrained  system  can  be  derived  by 
multiplying  the  two  sides  of  the  mathematical  model  found  in  Sect.  12.5.2 
by\7: 


rj-t  ••  rj-t  •  rj-t  rj-t 

L^Wa  +  =  l/r  -  L^S 

Further,  upon  writing  0  a  cis  a  linear  transformation  of  a  two-dimensional 
vector  u,  namely,  as 


K  =  Lu 


we  obtain 


L^ILii  +  L^CLu  =  17t  - 17 B 

and  hence,  the  generalized  inertia  matrix  under  pure  translation  is  L^IL. 

12.9  Find  the  maneuver(s)  under  which  the  Coriolis  and  centrifugal  forces  of  the 
robot  analyzed  in  Sect.  12.5.2  vanish.  Note  that  in  general,  these  forces  do  not 
vanish,  even  though  the  generalized  inertia  matrix  of  the  robot  is  constant. 

12.10  Find  the  eigenvalues  and  eigenvectors  of  the  matrix  of  generalized  inertia 
of  the  three-dof  rolling  robot  with  omnidirectional  wheels  analyzed  in 
Sect.  12.5.2. 

12. 1 1  The  Euler-Lagrange  equations  derived  for  holonomic  mechanical  systems  in 
Sect.  12.3,  termed  the  Euler-Lagrange  equations  of  the  second  kind,  require 
that  the  generalized  coordinates  describing  the  system  be  independent.  In 
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nonholonomic  mechanical  systems,  a  set  of  kinematic  constraints  is  not 
integrable,  which  prevents  us  from  solving  for  dependent  from  independent 
generalized  coordinates,  the  application  of  the  Euler-Lagrange  equations 
as  described  in  that  section  thus  not  being  possible.  However,  dependent 


generalized  coordinates  can  be  used  if  the  Euler-Lagrange  equations  of  the 


first  kind  are  recalled.  To  this  end,  we  let  q  be  the  m -dimensional  vector  of 
generalized  coordinates  that  are  subject  to  p  differential  constraints  of  the 
form 


A(q)q  =  b(q,  t) 


where  A  is  a  x  m  matrix  of  constraints  and  b  is  a  -dimensional  vector 
depending  on  the  configuration  q  and,  possibly,  on  time  explicitly.  When 
b  does  not  contain  t  explicitly,  the  constraints  are  termed  scleronomic; 
otherwise,  rheonomic.  Furthermore,  let  /7  =  m  —  p  ht  the  mobility  of  the 
system.  The  Euler-Lagrange  equations  of  the  first  kind  of  the  system  at  hand 
take  on  the  form 


where  A  is  a  -dimensional  vector  of  Lagrange  multipliers  that  are  chosen 
so  as  to  satisfy  the  kinematic  constraints.  Thus,  we  regard  the  m  dependent 
generalized  coordinates  grouped  in  vector  q  as  independent,  their  constraints 
giving  rise  to  the  constraint  forces  A . 

Use  the  Euler-Lagrange  equations  of  the  first  kind  to  set  up  the  mathe¬ 
matical  model  of  the  rolling  robot  with  omnidirectional  wheels  studied  in 


Sect.  12.5.2. 
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p.  93:  The  correct  matrix  [R2]c  is 


[R2]c  — 


0.373  -0.926  0.043 

0.902  0.352  -0.249 

0.215  0.132  0.967 


p.  129:  Line  below  Eq.  (3.133a)  :  in  light  of  Eq.  (2.39),  should 
read:  in  light  of  Eq.  (2.40) 

p.  137:  In  Exercise  3.20,  the  expression  for is  faulty. 
The  correct  expression  is 

=  Me  +  mPP^ 

p.  144:  The  last  line  of  text,  “One  thus  has,  using  subscripted  brack¬ 
ets  as  introduced  in  Sect.  2.2,”,  should  read : 

“One  thus  has,  using  subscripted  brackets  as  introduced  in  Sect.  2.3,” 


The  online  version  of  the  original  book  can  be  found  at 
http://dx.doi.org/10.1007/978-3-319-01851-5 
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Erratum 


p.  171:  The  third  line  of  text  below  eq.  (4.33),  “From  Defi¬ 
nition  2.2.1,  then  [u]l  =  [el]l  =  [e6]V\  should  read: 

“From  Definition  2.2.1,  then  [u]i  =  [07]  i  =  [e6]i” 


p.  178:  The  correct  expression  forQi23is 

0  10' 
1  0  0 
0  0  1  _ 

[e6]4  should  read: 


Qi23  —  Q1Q2Q3  — 


0-101  r  2/31  r  2/3 

[e6]4  =  (QiQ2Q3)^[e6]i  =  1  0  0  —2/3  =  2/3 

_0  O1JL-I/3J  L-1/3_ 

64^2  should  be 

6)42  =  -80.26438967° 

p.  182:  The  caption  of  Fig.  4.26  is  faulty.  The  correct 

caption  is  Motoman-EA1400N  welding  robot:  (a)  top  view;  (b)  side 
view;  (c)  orthographic  projection;  (d)  view  A,  as  per  side  view;  (e)  view 
B,  as  per  side  view.  All  dimensions  in  mm 

p.  200  Where  it  reads:  (b)  the  moments  of  the  three  lines  about  any  point 
on  the  intersecting  line  are  all  zero,  the  correct  wording  should  read : 

(b)  the  moments  of  the  three  lines  with  respect  to  the  intersecting  line  are 
all  zero. 

p.  202:  The  expression  fora  is  faulty.  The  correct  expre¬ 
ssion  is 


a  =  —= 

yal  + 

Please  refer  to  Appendix  A  for  details. 

p.  211:  Where  it  reads  :  with  and  defined  as  the  wrist  and  the  arm 
torques,  respectively,  the  correct  wording  should  read: 


with  ta  and  defined  as  the  arm  and  the  wrist  torques,  respectively. 
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p.  219:  Equation  (5.67c)  should  read: 


0i  —  (j)  —  (O2  O3) 


p.  291:  The  second  line  of  the  expression  for  L2  should 
read : 

-^m3a3(aiS23  +  2a2-?3)^i4  -  -  ^W3a2a3^3^| 

p.  321:  Caption  of  Fig.  7.7  should  read: 


Mass-center  location  of  the  robot  of  Fig.  4.17 

p.  324:  The  second  line  of  the  expression  for  tn  should 
read : 


ei 

ei  X  Pi  +  ei  X  Pi 


The  second  line  of  the  expression  for  t2i  should 
read : 


0 

ei  X  (coi  X  ai  +  0)2  X  P2) 


0 

(«/2)(i 


The  fourth  line  of  the  expression  for  t3i  should 
read : 


0 

(a/2)(i-3j) 


The  second  line  of  the  expression  for  t32  should 
read : 


pci  X  e2 

(pei  X  62)  X  (32  +  /O3)  +  62  X  [/l(ei  +  62)  X  32  +  p(ei  +  62  +  63)  X  P3] 


p.325:  Entry  (3,1)  of  matrix  T^MT  is  flawed.  The  correct 
expression  for  this  matrix  is: 


T^Mt  =  p 


—{\/2)a^m  —  I 
{\/A)a^m  +  I 
0 


-{\/A)a^m  {llA)a^m 

{\/A)a^m  0 

(3  /  A)a^m  (1  /  A)a^m  —  I 
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Erratum 


p.  326:  Entries  (1,3),  (2,3)  and  (3,1)  of  matrix  I  are  faulty.  The  correct  expression 
of  the  matrix  is: 


— (l/2)a^m  (5/4)(2^m 

(5/4)(2^m  0 

—I-\-{\/A)a^m  {\/2)a^m 


—I  +  {\/A)a^m 
{\/2)a^m 
0 


p.  327:  The  second- to- last  line  of  text, 
should  read: 


“Now  we  have”. 


“Now,  the  matrix  C  of  Coriolis  and  centrifugal  forces  is  obtained  as  shown 
below:” 


The  last  equation  displayed  should  read: 

C  =  T^MT  +  T^WMT  =  jcA 

p.  328  Entry  (1,1)  of  matrix  A  is  flawed.  The  correct 
expression  is 


—{\/A)a^m 
—{l/2)a^m  —  I 
{2/A)a^m  -h  I 


il I A)a^m  -h  I 
0 

{\/A)a^m  —  21 


—(l/2)a^m  —  21 
{l/4)a^m  -h  21 
0 


The  first  entry  of  the  vector  array  in  the  second 
equation  display  has  a  "(1/2)"  too  much.  The  cor¬ 
rect  display  is 


(T^Mt  -h  T^WMT)I9  = 


a^m  —  I 

-h  I 

a^m  —  I 


p.  329:  The  second  line  of  the  expression  f  or  C3  has  an  “=” 
too  much.  It  should  read: 

+<y3  X  (W3  X  Pj)  =  ]-ap^{-Ai  +  k)  -  -  j  +  k) 

The  expressions  for  n2  ,  and  ff  are  faulty.  They 
should  read: 


= 


=m2C2  +  ff  =  ]ramp^{-AS  +  k)  -  lamp^j  =  ^amp^(-S,j  +  k) 


^2  ~  ^2(^2  +  U)2  X  l2<^2  + 

+  ff 

6i— j— 8k) 


+  (32  -  P2)  X  f 


P 

3 


/2  i/M  n2  / 


a^mp^(—i+k) 


Ip^  (— i+j— k) +a^mp^  (i“2k) 
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=Ip^{-2i  +j  -  k)  +  -  12k) 

ff  =mici+ff  =  +  k) 

=  ^am/7^(i-9j  +  k) 

p.  330:  The  second  equation  display,  that  of  ri,  is  faulty 
The  correct  expression  reads: 

ri  =  nf  •  ei  =  +  a^rnp^ 

•  • 

The  first  component  of  vector  c(e,e)e  is  faulty 
The  correct  expression  is 


C(0,0)0  = 


—Ip^  +  a^mp^ 
Ip^  —  {\ / A)a^mp^ 
—Ip^  +  a^mp^ 
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FIGURE  1.  Elbow  singularity  of  the  Puma  robot 


E-6 


Erratum 


With  reference  to  the  figure  above,  the  relations  below  can  be  derived: 


IC 

a  = 

(1) 

02C 

1C  = 

yjal  +  bl 

(2) 

02C  = 

O2I  +  IC 

(3) 

021  = 

J a\-\- 

(4) 

Appendix  A 

Kinematics  of  Rotations:  A  Summary 


The  purpose  of  this  appendix  is  to  outline  proofs  of  some  results  in  the  realm  of 
kinematics  of  rotations  that  were  invoked  in  the  preceding  chapters.  Further  details 
are  available  in  the  literature  (Angeles  1988). 

We  start  by  noticing  two  preliminary  facts  whose  proof  is  straightforward,  as  the 
reader  is  invited  to  verify. 

Lemma  A.l.  The  {d /dt){-)  and  the  vect(  • )  operators,  for  3x3  matrix  operands, 
commute. 

and 

Lemma  A.2.  The  {d/dt){')  and  the  tr(-)  operators,  for  n  ^  n  matrix  operands, 
commute. 

Furthermore,  we  have 

Theorem  A.l.  Let  A  and  S  both  be  3  x3  matrices,  the  former  arbitrary,  the  latter 
skew -symmetric.  Then, 


vect(SA)  =  -[tr(A)l  — A]s 


where  s  =  vect(  S  ). 

Proof.  An  invariant  proof  of  this  theorem  appears  elusive,  but  a  componentwise 
proof  is  straightforward.  Indeed,  let  aij  denote  the  {i,  j)  entry  of  A,  and  si  the  /th 
component  of  s.  Then, 


SA  = 


-(221^3  +  ClwSl  -a22S3  +  -<3!23^3  +  (^33S2 

anS3  -  a3isi  <212^3  -  CI32S1  ai3S3  -  <233^1 
-anS2  +  ti2isi  -ai2S2  +  <222^1  -ai3S2  +  <223^1 


Hence, 
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(^22  +  a33)-5l  -  a\2S2  “ 

(flu  +  ai3)S2  -  a2\Si  -  a23S3 
(flu  +  a22)S3  -  a3iSi  -  a32S2_ 

On  the  other  hand, 


vect(SA)  =  - 


tr(A)l- A 


^22  +  ^33  “^12  ~^13 

—^221  an  +  ^233  —^223 

_  ~^3i  —a^i  an  +  ^22_ 


and  hence, 


i[tr(A)l-A]s=  i 

thereby  completing  the  proof.  Moreover,  we  have 

Theorem  A.2.  Lef  A,  S,  and  s  be  defined  as  in  Theorem  A.l.  Then, 

tr(  SA )  =  —2s  •  [vect(  A )] 


(^22  +  a33)‘?l  -  ai2^2  -  ai3S3 
{an  +  a33)s2  -  ^21^1  -  a23S3 

{an  +  a22)s3  -  ^23i'Si  -  (132^2 


Proof.  From  the  above  expression  for  SA, 


tr(SA)  =  -a2\S3  +  a31'?2  +  ^2l2'S3  -^232'Sl  -«13'?2  +  ^223'?1 

=  («23  -  a32)S\  +  (031  -  ai3)S2  +  {ai2  -  a2\)S3 


=  [^1  ^2  ^3] 


^23  ~  ^32 
^31  —  ^13 

a\2  —  a2i 


—2s  •  [vect(  A )] 


(A.l) 


q.e.d. 

Now  we  turn  to  the  proof  of  the  relations  between  the  time-derivatives  of  the 
rotation  invariants  and  the  angular- velocity  vector.  Thus, 

Theorem  A.3.  Let  v  denote  the  four-dimensional  array  of  natural  rotation  invari¬ 
ants,  as  introduced  in  Sect.  2.3.2  and  reproduced  below  for  quick  reference: 


V  = 


Then  the  relationship  between  v  and  the  angular  velocity  (o  is  given  by 
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V  =N(o 


with  N  defined  as 


[sin0/(2(l 


cos  (j)))]  (1  —  ee^)  —  (1  /  2)E 


Proof.  Let  us  obtain  first  an  expression  for  e.  This  is  readily  done  by  recalling  that 
e  is  the  real  eigenvector  of  Q,  i.e., 


Qe  =  e 


Upon  differentiation  of  both  sides  of  the  foregoing  equation  with  respect  to  time, 
we  have 


Qe  +  Qe  =  e 


i.e.. 


(1  -  Q)e  =  Qe 


An  expression  for  Q  can  be  derived  from  Eq.  (3.44),  which  yields 


Q  = 


(A.2) 


Therefore, 


Qe=S^e  =  a)xe 

and  hence,  the  above  equation  for  e  takes  the  form 

(1  —  Q)e  =  0)  X  e 

from  which  it  is  not  possible  to  solve  for  e  because  matrix  (1— Q)  is  singular.  Indeed, 
since  both  matrices  inside  the  parentheses  have  an  eigenvalue  + 1 ,  their  difference 
has  an  eigenvalue  0,  which  renders  this  difference  singular.  Thus,  one  more  relation 
is  needed  in  order  to  be  able  to  determine  e.  This  relation  follows  from  the  condition 
that  ||e|p  =  1.  Upon  differentiation  of  both  sides  of  this  condition  with  respect  to 
time,  we  obtain 


e^e  =  0 

the  last  two  equations  thus  yielding  a  system  of  four  scalar  equations  to 
determine  e.  We  now  assemble  these  equations  into  a  single  one,  namely. 
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Ae  =  b 


where  A  is  a  4  x  3  matrix,  while  b  is  a  four-dimensional  vector,  defined  as 


(o  xe 
0 


The  foregoing  overdetermined  system  of  four  equations  in  three  unknowns  now 
leads  to  a  system  of  three  equations  in  three  unknowns  if  we  multiply  its  two  sides 
by  A^  from  the  right,  thereby  producing 

A^Ae  =  A^b 


We  can  therefore  solve  for  e  from  the  above  equation  in  the  form 


e  =  (A^A)  ^A^b 


where  A^A  takes  the  form 

A^A  =  (2)1  -  (Q  +  Q^)  +  ee^ 

But  the  sum  inside  the  parentheses  is  readily  identified  as  twice  the  symmetric 
component  of  Q,  if  we  recall  the  Cartesian  decomposition  of  matrices  introduced 
in  Eq.  (2.57).  Therefore, 

Q  +  =  2[(cos  0)1  +  (1  —  cos  0)ee^] 


Hence, 


A^A  =  2(1  —  cos  0)1  —  (1  —  2  cos  0)ee 


T 


As  the  reader  can  readily  verify,  the  inverse  of  this  matrix  is  given  by 


(A^A)“^  = 


1 


1—2  cos  0  j. 
1  +  — - — ee 


2(1  —  cos  0)  2(1  —  cos  0) 


which  fails  to  exist  only  in  the  trivial  case  in  which  Q  becomes  the  identity  matrix. 
Upon  expansion  of  the  last  expression  for  e,  we  have 


1 


e  =  — 


(E  -  Q^E)<w 


2(1  —  cos  0) 


Now  Q^E  is  obtained  by  recalling  Eq.  (2.55),  thereby  obtaining 


Q^E  =  (cos0)E  +  (sin0)(l  —  ee^) 
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the  final  expression  for  e  thus  being 

1 


e  =  — 


[(1  —  cos  (p)E  —  (sin0)(l  —  ee^)]a) 


2(1  —  COS0) 


Now,  an  expression  for  0  is  obtained  upon  equating  the  trace  of  the  two  sides  of 
Eq.  (A.2),  which  yields 


tr(Q)  =  tr(aQ) 


(A.3) 


From  Lemma  A.2, 


tr(Q)  =  ^tr(Q) 


(A.4) 


and  hence. 


tr(  Q  )  =  —20  sin  0 
On  the  other  hand,  from  Theorem  A.2, 


tr(S^Q)  =  —2(0  •  (sin0)e 


Therefore, 


(p  =  (o  •  e 


Upon  assembling  the  expressions  for  e  and  0,  we  obtain  the  desired  relation,  with 
N  given  as  displayed  above,  thereby  proving  the  theorem. 

Theorem  A.4.  Let  X  denote  the  four-dimensional  array  of  linear  rotation  invari¬ 
ants,  as  introduced  in  Sect.  2.3.3  and  reproduced  below  for  quick  reference: 

X  = 

Then  the  relationship  between  X  and  the  angular  velocity  is  given  by 

X  =  h(o 


(sin0)e 

vect( Q ) 

COS0 

.[tr(Q)-l]/2. 

with  L  defined  as 


L  = 


■(l/2)[tr(Q)l-Q]' 
— (sin0)e^ 
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Proof.  From  Lemma  A.  1 ,  we  have 


^vect(Q)  =  vect(Q)  (A.5) 

at 


On  the  other  hand,  equating  the  vectors  of  the  two  sides  of  Eq.  (A.2)  yields 


vect(  Q )  =  vect(  Q ) 


and  hence, 


d 

vect(Q)  =  vect(S^Q) 
dt 

But,  if  we  recall  Theorem  A.l,  the  foregoing  relation  leads  to 

^vect(Q)  =  i[tr(Q)l-Q]ft) 
Likewise,  from  Lemma  A.2,  we  have 

^tr(Q)  =  tr(Q) 

and  hence, 

^tr(Q)  =  tr(flQ) 

Now,  if  we  recall  Theorem  A.2,  the  foregoing  relation  leads  to 

— tr(Q)  =  —2(0  •  [vect(Q)]  =  — 2(sin0)e^a) 
dt 


Hence, 


'T' 

—  (cos0)  =  — (sin0)e  (o 
dt 

Upon  assembling  the  last  two  expressions  for  the  time-derivatives  of  the  vector  of 
Q  and  cos  (j),  we  obtain  the  desired  relation. 

Theorem  A.5.  Let  rj  denote  the  four-dimensional  array  of  the  Euler-Rodrigues 
parameters  of  a  rotation,  as  introduced  in  Sect.  2.3.6  and  reproduced  below  for  quick 
reference: 
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■[sin(0/2)]e' 

r 

COS  (0/2) 

7o_ 

Then,  the  relationship  between  rj  and  the  angular  velocity  takes  the  form 


with  H  defined  as 


1 

cos(0/2)l  —  sin(0/2)E 

1 

1 — 

1 

o 

1 _ 

2 

—  sin(0/2)e^ 

^  2 

T 

— r 

where  R  is  the  cross-product  matrix  ofr. 
Proof.  If  we  differentiate  r,  we  obtain 


0 


0 


+  e—  cos  — 


and  hence,  all  we  need  to  derive  the  desired  relations  is  to  find  expressions  for  e 
and  0  in  terms  of  the  Euler-Rodrigues  parameters.  Note  that  from  Theorem  A. 3,  we 
already  have  those  expressions  in  terms  of  the  natural  invariants.  Thus,  substitution 
of  the  time-derivatives  of  the  natural  invariants,  as  given  in  Theorem  A. 3,  into  the 
above  expression  for  r  leads  to 


sin0 
1  —  cos  0 


(O 


sin0 
1  —  cos  0 


e 


(A.6) 


Now,  by  recalling  the  identities  giving  the  trigonometric  functions  of  0  in  terms  of 
those  of  0/2,  we  obtain 


sin 


sm0  { 

-  =  cos  — 

1  —  cos  0  V  ^  / 


and  hence,  the  term  in  brackets  of  the  above  expression  vanishes,  the  expression  for 
r  thus  reducing  to 


1 

2 


(rol  -  R)<w 


thereby  completing  the  proof. 
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The  numerical  solution  of  the  most  common  types  of  systems  of  linear  and  nonlinear 
equations  is  outlined  in  this  appendix. 

We  start  with  the  solution  of  the  linear  algebraic  system 

Ax  =  b  (B.l) 

with  A  defined  as  2i  full-rank  mxn  matrix,  while  x  and  b  are  /7-  and  m -dimensional 
vectors,  respectively.  The  case  m  =  n  is  the  most  frequently  encountered;  this  case 
was  discussed  in  Sect.  5.2  and  need  not  be  further  discussed.  We  will  consider  only 
two  cases: 

(a)  overdetermined:  m  >  n  \  and 

(b)  underdetermined:  m  <  n. 

The  overdetermined  case  does  not  admit  a  solution,  unless  vector  b  happens  to  lie 
in  the  range  of  A.  Besides  this  special  case,  then,  we  must  reformulate  the  problem; 
rather  than  seeking  a  solution  of  Eq.  (B.l),  we  will  look  for  an  approximation  of  that 
system  of  equations.  Moreover,  we  will  seek  an  approximation  that  will  satisfy  an 
optimality  condition. 

The  underdetermined  case,  on  the  contrary,  admits  infinitely  many  solutions,  the 
objective  then  being  to  seek  one  that  satisfies  (a)  the  system  of  equations  and  (b)  an 
additional  optimality  condition  as  well. 

A  nonlinear  system  of  equations  takes  the  form 

f(x)  =  Om 

where  f  and  x  are  m-  and  /7 -dimensional  vectors,  respectively,  while  0^  is  the 
m -dimensional  zero  vector.  We  consider  the  cases: 

(a)  determined:  m  =  n;  and 

(b)  overdetermined:  m  >  n. 
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The  underdetermined  case  m  <  n,  not  occurring  in  the  book,  and  requiring 
specialized  knowledge  of  mathematical  programming,  is  left  out. 

We  study  each  of  the  two  cases  above  in  the  sections  below. 


B.l  The  Overdetermined  Linear  Case 


The  error  e  in  the  approximation  of  Eq.  (B.l)  is  defined  as 

e  =  b  —  Ax  (B.2) 


An  obvious  way  of  imposing  an  optimality  condition  on  the  solution  x  is  to 
specify  that  this  solution  minimize  a  norm  of  e.  All  norms  of  e  can  be  expressed  as 


e 


(B.3) 


with  Ck  being  understood  as  the  kth  component  of  the  m -dimensional  vector  e. 
When  p  =  2,  the  foregoing  norm  is  known  as  the  Euclidean  norm,  which  we  have 
used  most  frequently  in  this  book.  When  p  ^  oo,  the  infinity  norm,  also  known  as 
the  Chebyshev  norm,  is  obtained.  It  turns  out  that  upon  seeking  the  value  of  x  that 
minimizes  a  norm  of  e,  the  simplest  is  the  Euclidean  norm,  for  the  minimization 
of  its  square  leads  to  a  linear  system  of  equations  whose  solution  can  be  obtained 
directly,  as  opposed  to  iteratively.  Indeed,  let  us  set  up  the  minimization  problem 
below: 


min 

X 


(B.4) 


The  normality  condition  of  the  minimization  problem  at  hand  is  derived  upon  setting 
the  gradient  of  z  with  respect  to  x  equal  to  zero,  i.e.. 


dz 

dx 


Now,  the  chain  rule  and  the  results  of  Sect.  2.3.1  allow  us  to  write 


(B.5) 


dz 

dx 


de 


(B.6) 


and  hence,  we  have  the  first  result: 

Theorem  B.l.  The  error  in  the  approximation  ofEq.  (B.l),  for  the  full-rank  m  x  n 
matrix  A,  with  m  >  n,  is  of  minimum  Euclidean  norm  if  it  lies  in  the  null  space 

ofPJ: 


B  Numerical  Equation- Solving 


557 


A^e  =  0„  (B.7) 

Furthermore,  if  Eq.  (B.2)  is  substituted  into  Eq.  (B.6),  and  the  product  thus 
resulting  is  substituted,  in  turn,  into  the  normality  condition,  we  obtain 

A^Ax  =  A^b  (B.8) 

which  is  known  as  the  normal  equation  of  the  minimization  problem  at  hand.  By 
virtue  of  the  assumption  on  the  rank  of  A,  the  product  A^A  is  positive -definite  and 
hence,  invertible.  As  a  consequence,  the  value  xq  of  x  that  minimizes  the  Euclidean 
norm  of  the  approximation  error  of  the  given  system  is 


Xo  =  (A^A)“‘A^b  (B.9) 

the  matrix  coefficient  of  b  being  known  as  a  generalized  inverse  of  A.  The  error 
obtained  with  this  value  is  known  as  the  least-square  error  of  the  approximation, 
i.e.. 


eo  =  b  — Axo  (B.IO) 

The  reader  should  be  able  to  prove  one  more  result: 

Theorem  B.2  (Projection  Theorem).  The  least-square  error  eo  is  orthogonal 

to  Axq. 

While  the  formula  yielding  the  foregoing  generalized  inverse  is  quite  simple  to 
implement,  the  number  of  floating-point  operations  (flops)  it  takes  to  evaluate, 
along  with  the  ever-present  roundoff  errors  in  both  the  data  and  the  results, 
renders  it  not  only  inefficient,  but  also  unreliable  if  applied  as  such.  Indeed,  if 
we  recall  the  concept  of  condition  number,  introduced  in  Sect.  5.8  and  recalled  in 
Sect.  9.4.1,  along  with  the  definition  adopted  in  the  latter  for  the  condition  number, 
it  becomes  apparent  that  the  condition  number  of  A^  A  is  exactly  the  square  of  the 
condition  number  of  A.  This  result  can  be  best  understood  if  we  apply  the  Polar- 
Decomposition  Theorem,  introduced  in  Sect.  5.8,  to  rectangular  matrices,  but  we 
will  not  elaborate  on  this  issue  here. 

As  a  consequence,  then,  even  if  A  is  only  slightly  ill-conditioned,  the  product 
A^A  can  be  catastrophically  ill-conditioned.  Below  we  outline  two  procedures  to 
calculate  efficiently  the  least-square  approximation  of  the  overdetermined  system 
(B.l)  that  preserve  the  condition  number  of  A  and  do  this  with  a  low  number  of 
flops  (floating-point  operations). 
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B.l.l  The  Numerical  Solution  of  an  Overdetermined  System 
of  Linear  Equations 


In  seeking  a  numerical  solution  of  the  system  of  equations  at  hand,  we  would 
like  to  end  up  with  a  triangular  system,  similar  to  the  LU-decomposition  applied 
to  solve  a  system  of  as  many  equations  as  unknowns,  and  hence,  we  have  to 
perform  some  transformations  either  on  the  rows  of  A  or  on  its  columns.  A  safe 
numerical  procedure  should  thus  preserve  (a)  the  Euclidean  norm  of  the  columns 
of  A  and  (b)  the  inner  product  between  any  two  columns  of  this  matrix.  Hence,  a 
triangularization  procedure  like  LU-decomposition  would  not  work,  because  this 
does  not  preserve  inner  products.  Obviously,  the  transformations  that  do  preserve 
these  inner  products  are  orthogonal,  either  rotations  or  reflections.  Of  these,  the  most 
best-known  methods  are  (a)  the  Gram-Schmidt  ortho gonalization  procedure  and 
(b)  Householder  reflections. 

The  Gram-Schmidt  procedure  consists  in  regarding  the  columns  of  A  as  a  set  of 
n  m -dimensional  vectors  {  From  this  set,  a  new  set  {  is  obtained  that  is 
orthonormal.  The  procedure  is  quite  simple  and  works  as  follows:  Define  ei  as 


ei  = 


ai 


(B.ll) 


Further,  we  define  02  as  the  normal  component  of  a2  onto  02,  as  introduced  in 
Eq.  (2.6b),  i.e.. 


a^  =  (1  -0i0[)a2 


02  = 


(B.12a) 

(B.12b) 


In  the  next  step,  we  define  03  as  the  unit  vector  normal  to  the  plane  defined  by 
01  and  02  and  in  the  direction  in  which  the  inner  product  0^a3  is  positive,  which 
is  possible  because  all  vectors  of  the  set  {  a^  Yi  have  been  assumed  to  be  linearly 
independent — remember  that  A  has  been  assumed  to  be  of  full  rank.  To  this  end,  we 
subtract  from  a3  its  projection  onto  the  plane  mentioned  above,  i.e.. 


a^  =  (1  -0i0[  -020[)a3 


03  = 


a 


a 


(B.13a) 

(B.13b) 


and  so  on,  until  we  obtain  0„-i,  the  last  unit  vector  of  the  orthogonal  set,  0„,  being 
obtained  as 


a;|-  =  (1  -  eie[  -  eaej - e„_iej_i)a„ 


(B.14a) 
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Finally, 


(B.14b) 


In  the  next  stage,  we  represent  all  vectors  of  the  set  {  in  orthogonal  coordinates, 
i.e.,  in  the  base  (9  =  {  which  are  then  arranged  in  an  m  x  ^  array  By  virtue 
of  the  form  in  which  the  set  {  ”  was  defined,  the  last  m—k  components  of  vector 

vanish.  We  thus  have,  in  the  said  orthonormal  basis. 


Otlk 


[^k]o  = 


t^kk 

0 


0 


Further,  we  represent  b  in  O  as  well,  thus  obtaining 


Therefore,  Eq.  (B.l),  when  expressed  in  O,  becomes 


(B.15a) 


(B.15b) 


ail  0ii2  •  •  •  Oiin 

■  Pi  ■ 

0  0^22  •  •  •  Oiln 

Pi 

• 

Xi 

• 

. 

X2 

• 

0  0  •  •  • 

• 

— 

Pn 

0  0  •••  0 

• 

Pn  +  \ 

•  •  •  • 

-Xn_ 

• 

1 

•  o 

• 

• 

• 

•  o 

•  o 

_ 1 

_  _ 

(B.16) 


whence  x  can  be  computed  by  back-substitution.  It  is  apparent,  then,  that  the  last 
m  —  n  equations  of  the  foregoing  system  are  incompatible:  their  left-hand  sides 
are  zero,  while  their  right-hand  sides  are  not  necessarily  so.  What  the  right-hand 
sides  of  these  equations  represent,  then,  is  the  approximation  error  in  orthogonal 
coordinates.  The  error  Euclidean  norm  is,  then. 
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(B.17) 


The  second  method  discussed  here  is  based  on  the  application  of  a  chain  of  n 
reflections  {H^  known  as  Householder  reflections,  to  both  sides  of  Eq.  (B.l). 
The  purpose  of  these  reflections  is,  again,  to  obtain  a  representation  of  matrix  A 
in  upper- triangular  form  (Golub  and  Van  Loan  1989).  The  algorithm  proceeds  as 
follows:  We  assume  that  we  have  applied  reflections  Hi ,  H2, . . .,  H^-i ,  in  this  order, 
to  A  that  have  rendered  it  in  upper-trapezoidal  form,  i.e.. 


A,_i  ^H,_i  .  .  .H2HiA 


* 

1 _ 

«12 

«u-l 

>|c 

1 - 

• 

• 

• 

0 

^22 

•••  </-l 

>fc 

•••  «2« 

0 

0 

^3i 

0 

0 

•  •  • 

a  1  ■ 

•••  <-!,« 

0 

0 

0 

a  -  ■ 

^1,1 

•  •  •  a 

^i,n 

1 

0  •  • 

0 

0 

a  ■ 

•  •  •  a 

^mn  -1 

(B.18) 


The  next  Householder  reflection,  H, ,  is  determined  so  as  to  render  the  last  m  —  i 
components  of  the  ith  column  of  A^  -i  equal  to  zero,  while  leaving  its  first  i  —  1 
columns  unchanged.  We  do  this  by  setting 


Oii 

U, 


)  y  Ky  +  («r+i.;)^  +  •  •  •  +  «,)2 

0  0...0<. 


=  1-2 


U/U 


T 


I  "i 


u, 


where  sgn(v)  is  defined  as  +1  if  x  >  0,  as  —1  if  x  <0,  and  is  left  undefined  when 
X  =  0.  As  the  reader  can  readily  verify. 


1 

2 


=  ai(ui)i  =  O'/ (a*.  +  ai)  =  yi 


and  hence,  the  denominator  appearing  in  the  expression  for  H/  is  calculated  with 
one  single  addition  and  one  single  multiplication.  It  is  noteworthy  that  H/ ,  as  defined 
above,  is  the  n  xn  counterpart  of  the  3x3  pure  reflection  defined  in  Eq.  (2.5).  As  a 
matter  of  fact,  H/  reflects  vectors  in  m -dimensional  space  onto  a  hyperplane  of  unit 
normal  U;/||u/ 1|. 
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It  is  important  to  realize  that 

(a)  at  is  defined  with  the  sign  of  a*-  because  yi  is  a  multiple  of  the  i th  component 
of  U; ,  which  is,  in  turn,  the  sum  of  a*-  and  ,  thereby  guaranteeing  that  the 
absolute  value  of  this  sum  will  always  be  greater  than  the  absolute  value  of 
each  of  its  terms.  If  this  provision  were  not  made,  then  the  resulting  sum  could 
be  of  a  negligibly  small  absolute  value,  which  would  thus  render  yi  a  very 
small  positive  number,  thereby  introducing  unnecessarily  an  inadmissibly  large 
roundoff-error  amplification  upon  dividing  the  product  U/  uf  by  yt ; 

(b)  an  arbitrary  vector  v  is  transformed  by  H/  with  unusually  few  flops,  namely, 

1 

H,  v  =  v - (v  u,)u/ 

7/ 

Upon  application  of  the  n  Householder  reflections  thus  defined,  the  system  at 
hand  becomes 


HAx  =  Hb 


(B.19) 


with  H  defined  as 


(B.20) 


Similar  to  that  in  equation  (B.16),  the  matrix  coefficient  of  x  in  Eq.  (B.19),  i.e.,  HA, 
is  in  upper- triangular  form.  That  is,  we  have 


HA  = 


(B.21) 


with  Om'n  denoting  the  (m  —  n)  x  n  zero  matrix,  m'  =  m  —  n,  and  hu  and  hi 
being  n-  and  m' -dimensional  vectors.  The  unknown  x  can  thus  be  calculated  from 
Eq.  (B.19)  by  back-substitution. 

Note  that  the  last  m'  components  of  the  left-hand  side  of  Eq.  (B.19)  are  zero, 
while  the  corresponding  components  of  the  right-hand  side  of  the  same  equation  are 
not  necessarily  so.  This  apparent  contradiction  can  be  resolved  by  recalling  that  the 
overdetermined  system  at  hand  in  general  has  no  solution.  The  lower  part  of  b, 
is  then  nothing  but  an  m' -dimensional  array  containing  the  nonzero  components  of 
the  approximation  error  in  the  new  coordinates.  That  is,  the  least-square  error,  Cq,  in 
these  coordinates  takes  the  form 


eo 


bL 


(B.22a) 


Therefore, 


lleoll  = 


(B.22b) 
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B.2  The  Underdetermined  Linear  Case 

In  this  section  we  study  the  solution  of  system  (B.l)  under  the  assumption  that 
m  <  n  and  rank(A)  =  m.  Now,  the  system  under  study  admits  infinitely  many 
solutions,  which  allows  us  to  impose  one  condition  on  a  specific  solution  that  we 
may  want  to  obtain.  The  obvious  choice  is  a  minimality  condition  on  a  norm  of  x. 
As  in  the  previous  section,  the  minimization  of  the  square  of  the  Euclidean  norm  of 
X  leads  to  a  linear  problem,  and  hence,  a  direct  solution  of  the  problem  at  hand  is 
possible.  We  thus  have 


min 

X 


(B.23) 


subject  to  the  constraint  represented  by  Eq.  (B.l).  Since  we  now  have  a  constrained 
minimization  problem,  we  proceed  to  its  solution  via  Lagrange  multipliers.  That  is, 
we  introduce  a  new  objective  function  ^  (x),  defined  as 


^(x)  =  z(x)  +  A^(Ax  —  b)  ^  min  (B.24) 

x,A 


subject  to  no  constraints,  with  X  defined  as  an  m -dimensional  vector  of  Lagrange 
multipliers,  as  yet  to  be  determined.  We  thus  have  now  an  unconstrained  mini¬ 
mization  problem  with  m  -\-  n  design  variables,  the  m  components  of  X  and  the  n 
components  of  x,  that  we  group  in  the  (m  +  /i) -dimensional  vector  y  =  [ x^  X^  Y . 
The  normality  condition  of  the  foregoing  problem  can  now  be  stated  as 


dl; 

dy 


m+n 


(B.25a) 


with  defined  as  the  (m  +  /i) -dimensional  zero  vector.  The  above  condition  can 

be  broken  down  into  the  two  conditions  below: 


dl; 

dx 


dt; 

dX 


with  0^  and  defined,  respectively,  as  the  m-  and  /7 -dimensional  zero  vectors.  The 
above  equations  thus  lead  to 


^  =  X  +  A^A  = 
dx 


d^ 

dX 


=  Ax  —  b  =  0^ 


(B.26) 


(B.27) 
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Upon  elimination  of  A  from  the  above  system  of  equations,  we  obtain 

X  =  A^(AA^)“‘b  (B.28) 

which  is  the  minimum-norm  solution  of  the  proposed  problem.  Again,  the  formula 
yielding  the  foregoing  solution  is  deceptively  simple.  If  we  attempt  the  calculation 
of  the  inverse  occurring  in  it,  we  risk  introducing  unnecessarily  an  inadmissibly 
ill-conditioned  matrix,  the  product  AA^.  Therefore,  an  alternative  approach  to  the 
straightforward  implementation  of  the  above  formula  should  be  attempted,  as  we  do 
in  the  subsection  below. 


B.2.1  The  Numerical  Solution  of  an  Underdetermined  System 
of  Linear  Equations 


The  simplest  way  of  solving  this  problem  is  by  introducing  the  mxm  identity  matrix 
1,  in  a  disguised  manner,  between  the  two  factors  of  the  left-hand  side  of  Eq.  (B.l). 
To  this  end,  we  assume  that  we  have  an  orthogonal  mxm  matrix  H,  so  that 


H^H  =  1 


Equation  (B.l)  thus  becoming 


AH^Hx  =  b 


which  can  be  rewritten  in  the  form 


AH^v  =  b 


with  V  defined,  obviously,  as 


V  =  Hx 


(B.29) 


(B.30a) 


(B.30b) 


(B.30c) 


Now,  H  is  chosen  as  the  product  of  m  Householder  reflections  that  transforms 
A^  into  upper-triangular  form,  i.e.,  so  that 


HA^ 


U 


n  m 


(B.31) 


with  On'm  defined  as  the  n'  x  m  zero  matrix  and  n'  =  n  —  m.  Moreover,  U  is 
upper-triangular.  Eurther,  let  us  partition  v  in  the  form 
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V[/ 

Vl 


(B.32) 


Upon  substitution  of  Eqs.  (B.31)  and  (B.32)  into  Eq.  (B.30b),  we  obtain 

H  =  b 

where  Omn'  is  the  m  xn'  zero  matrix.  Hence, 

U^Vf/  +  Om„>\L  =  b  (B.33) 

whence  it  is  apparent  that  \l  can  attain  any  value.  Now,  since  v  is  the  image  of  x 
under  an  orthogonal  transformation,  the  Euclidean  norms  of  these  two  vectors  are 
identical,  and  hence. 


[U^  Omn'] 


=  \Wuf  +  II  Vl 


(B.34) 


Therefore,  if  we  want  to  minimize  the  Euclidean  norm  of  x,  the  obvious  choice  of 
Vl  is  zero.  Eurthermore,  from  Eq.  (B.33), 

Vf/  =  U“^b  (B.35) 


and  so. 


X  =  H^v  = 


U-^b' 


(B.36) 


with  denoting  the  /7 '-dimensional  zero  vector,  thereby  completing  the  numerical 
solution  of  the  problem  at  hand. 


B.3  Nonlinear-Equation  Solving:  The  Determined  Case 

Definition  B.3.1.  A  system  of  algebraic  equations  containing  some  that  are  not 
linear  is  termed  nonlinear.  If  the  number  of  equations  is  identical  to  the  number 
of  unknowns,  the  system  is  determined. 

Example.  Eind  the  intersection  of  the  circle  and  the  hyperbola  depicted  in  Eig.  B .  1 . 
Solution:  The  equations  of  the  circle  and  the  hyperbola  are 


/i(x,  j)  =  +  y^  -4  =  0 

f2(x,y)  =  x^  -y^  -I  =  0 
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Fig.  B.l  Intersection  of  a 
circle  and  a  hyperbola 


Table  B.l  The  four 
intersection  points  of  the 
circle  and  the  hyperbola 
of  Fig.  B.l 


The  solution  to  a  nonlinear  system  of  equations,  when  one  exists  at  all,  is  usually 
multiple:  The  circle  and  the  hyperbola  of  Fig. B.l  intersect  at  four  points 
of  coordinates  (x/,  j/),  as  displayed  in  Table  B.l.  The  problem  may  have  no  real 
solution,  e.g.,  the  circle  and  the  hyperbola  of  Fig.  B.2  do  not  intersect.  The  system  of 
equations  from  which  the  coordinates  of  the  intersection  points  are  to  be  computed 
is  given  below: 


+  y^  -I  =  0 
fiix,  y)  =  x^  -  y^  -  16  =  0 

This  system  of  equations  admits  no  real  solution! 

In  general,  a  determined  nonlinear  system  of  equations  takes  the  form 


f(x)  =  0 


(B.37) 
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Fig.  B.2  A  circle  and  a 
hyperbola  that  do  not 
intersect 


X 


where  x  and  f  are  both  -dimensional  vectors: 


X\ 

f\  1  •>  X2  ? 

.  .  .  ,  Xyi) 

X  = 

X2 

,  f  = 

J2  (-^1 J  X2 1 

.  .  .  ,  Xyi^ 

(B.38) 

-Xn_ 

^fn  »  X2  ? 

.  .  .  ,  Xfi^ _ 

B.3.1  The  Newton-Raphson  Method 

We  outline  below  the  method  of  solution  of  determined  nonlinear  systems  using 
the  Newton-Raphson  method.  This  is  an  iterative  method,  whereby  a  sequence  of 
approximations  is  obtained  that,  if  converging,  approaches  the  solution  in  a  finite 
number  of  iterations  within  a  prescribed  tolerance. 

A  value  of  x  is  given  as  an  initial  guess: 

=  [Pl  P2  ■■■  Pn] 


and  f  is  evaluated  at  x^: 


=  f(x^) 

If  the  value  x^  was  chosen  randomly,  most  likely  it  will  not  verify  the  given  system 
of  equations,  i.e.. 
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Next,  we  look  for  a  “small”  increment  Ax  of  x  (the  increment  is  small  if  its  norm — 
any  norm — is  small): 


Ax  =  Axi  Ax2  . 


T 


Now,  f(x^  +  Ax)  is  evaluated  up  to  its  linear  approximation  (all  quadratic  and  higher- 
order  terms  are  dropped  from  its  series  expansion): 


f(x°  +  Ax) 


«  f(x®)  + 


9f 

9x 


X=xO 


Ax 


(B.39) 


The  Jacobian  matrix  of  f  with  respect  to  x  is  defined  as  the  matrix  of  partial 
derivatives  of  the  components  of  f  with  respect  to  all  the  components  of  x: 


9/i/9xi  9/i/9x2  •••  9/i/9x„ 

9/2/9X1  9/2/9X2  •••  9/2/9x„ 


9/„/9xi  9/„/9x2  •••  9/«/9x„ 


(B.40) 


In  the  next  step,  we  find  Ax  that  renders  zero  the  linear  approximation  of  f(xo  + 
Ax): 


fo  +  F(x°)Ax  =  0 


or 


F(x°)Ax  = 


(B.41) 


whence  Ax  can  be  found  using,  for  example,  Gaussian  elimination: 

Ax  =  -F^‘f°,  Fo  =  F(x°)  (B.42) 


Next,  X  is  updated: 


X 


x°  +  Ax 


the  procedure  stopping  when 


II^Xll  < 


(B.43) 


(B.44) 


for  a  prescribed  tolerance  Cx . 
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Remarks. 

•  Use  the  maximum  norm  to  test  convergence  in  Eq.  (B.44),  for  it  costs  virtually 
nothing; 

•  no  guarantee  that  the  Newton-Raphson  method  will  converge  at  all; 

•  whether  the  Newton-Raphson  method  converges  is  dependent  upon  the  initial 
guess,  x^; 

•  the  boundary  between  regions  of  convergence  and  divergence  is  a  fractal 
(Mandelbrot  1983;  Gleick  1988); 

•  when  the  Newton-Raphson  method  converges,  it  does  so  quadratically:  At  every 
iteration,  two  decimal  places  of  accuracy  are  gained  (Dahlquist  and  Bjorck  1974). 


B.4  Overdetermined  Nonlinear  Systems  of  Equations 

A  system  of  nonlinear  equations  of  the  form 

f(x)  =  0  (B.45) 

where  x  is  a  -dimensional  vector  and  f  is  a  ^-dimensional  vector,  is  overdetermined 
if  q  >  n.  Just  as  in  the  linear  case,  in  general,  no  vector  x  can  be  found  that  verifies 
all  the  q  scalar  equations  of  the  system.  However,  approximations  can  be  found  that 
minimize  the  least- square  error  of  the  approximation,  as  described  in  the  balance  of 
this  Section.  The  method  of  solution  adopted  here  is  the  overdetermined  counterpart 
of  the  Newton-Raphson  method. 


B.4.1  The  Newton-Gauss  Method 


Problem.  Find  an  approximate  solution  to  system  (B.45)  that  verifies  those  equa¬ 
tions  with  the  least-square  error. 


1  ^ 

F(x)  =  -f^Wf 


mm 

X 


(B.46) 


where  W  is  a  ^  x  ^  positive-definite  weighting  matrix. 

Solution:  We  follow  a  procedure  similar  to  Newton-Raphson’ s,  which  is  known 
as  the  Newton-Gauss  method,  as  described  below: 

First,  an  initial  guess  x^  of  x  is  given;  then,  we  produce  the  sequence 


X 


1 


•  9 


(B.47) 


such  that 


+  Ax^ 


(B.48) 
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Calculation  of  Ax^ : 

•  Factor  W  into  its  two  Cholesky  factors: 


W  =  V^V  (B.49) 

which  is  possible  because  W  is  assumed  positive-definite. 

•  Compute  Ax^  as  the  least-square  solution  of  the  unconstrained  overdetermined 
linear  system 


VF(x^)Ax^  =  -Vf(Ax^) 


(B.50) 


with  F(x)  defined  as  the  q  x  n  Jacobian  matrix  of  the  vector  function  f(x),  i.e., 


(B.51) 


Dropping  superscripts  for  the  sake  of  notation- simplicity  and  recalling  Eqs.  (B.7) 
and  (B.8), 


Ax  =  -(F^WF)“^F^Wf  (B.52) 

This  procedure  is  iterative,  stopping  when  a  convergence  criterion,  discussed  in 
Sect.  B.4.2,  is  met. 


B,4.2  Convergence  Criterion 


Calculate  first  VF(x): 


VF(x) 

af 

9x 


dF 

9x 

F, 


9f\^  dF 

9x }  9f 

(B.53) 

=  Wf 

(B.54) 

Hence,  the  condition  for  a  stationary  point  is 


F^Wf  =  0 


(B.55) 


which  is  the  normality  condition  of  Problem  (B.46). 

It  is  thus  apparent  that,  at  a  stationary  point  of  F,  f(x)  need  not  vanish;  however, 
f(x)  must  lie  in  the  null  space  of  F^W.  Moreover,  from  Eqs.  (B.52)  and  (B.55) 
follows  that,  at  a  stationary  point.  Ax  vanishes.  Hence,  the  convergence  criterion  is 
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II  Axil  <e 


(B.56) 


where  6  is  a  prescribed  tolerance. 

Remarks. 

•  The  normality  condition  (B.55)  alone  does  not  guarantee  a  minimum,  but  only  a 
stationary  point. 

•  However,  as  it  turns  out,  if  the  procedure  converges,  then  it  does  so,  to  a  second- 
order  approximation,  to  a  minimum,  and  neither  to  a  maximum  nor  a  to  saddle 
point,  as  we  prove  below. 

The  sequence  T'(x^),  T'(x^),  . . . ,  F(x^),  F(x^“^^),  . . . ,  obtained  from  the  sequence 

of  X  values,  evolves,  to  a  first  order,  as  AF(x),  given  by 


AF 


(B.57) 


i.e. 


AF  =  f^WFAx 


(B.58) 


Upon  plugging  expression  (B.52)  of  Ax  into  eq.  (B.58),  we  obtain 

A/  =  -  f  WF(F^WF)“'F^Wf  =  -f^Mf  (B.59) 

" - V - " 

M 


where,  apparently,  M  is  a  ^  x  ^  positive-definite  matrix.  As  a  consequence,  f^Mf 
becomes  a  positive-definite  quadratic  expression  of  f;  hence,  AF  is  negative  defi¬ 
nite.  Thus,  the  second-order  approximation  of  F(x)  is  negative-definite,  and  hence, 
the  sequence  of  F  values  decreases  monotonic  ally .  That  is,  in  the  neighbourhood  of 
a  stationary  point  the  first-order  approximation  of  f(x)  is  good  enough,  and  hence, 
if  the  procedure  converges,  it  does  so  to  a  minimum. 

The  reader  may  wonder  whether  the  Newton-Raphson  method  can  be  used 
to  solve  nonlinear  least-square  problems.  Although  the  answer  is  yes,  the 
Newton-Raphson  method  is  not  advisible  in  this  case,  as  made  apparent  below. 

Recall  VF  from  Eqs.  (B.46)  and  (B.47): 


VF(x)  = 


3F 

9x 


=  F^(x)^^  f(x) 

nx-q  dim 

T 


VF(x)  =  0  ^  (x)Wf(x)  =  0 


=  '^(x)GR” 


(NC) 
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thereby  obtaining  a  determined  system  of  n  equations  in  n  unknowns.  This  system 
can  be  solved  using  Newton-Raphson  method  which  requires  V  ^  (x) : 


Wilf(x)  = 


di[r 

9x 


-[F^(x)  Wf(x)] 


(af/axiT’ 


That  is,  V ifr  (x)  involves  second-order  derivatives  of  ifr  with  respect  to  x: 


dxj  dxi  ’ 


In  summary,  the  Newton-Raphson  method  is  too  cumbersome  and  prone  to 
ill-conditioning,  for  it  is  based  on  the  normality  conditions  of  the  problem  at  hand. 
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of  serial  manipulators,  206 
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Appendix  A,  547 
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Chasles’  Theorem,  see  Mozzi-Chasles’ 
Theorem 

Chebyshev  norm,  228, 497 
Cholesky-decomposition  algorithm,  318,  338 
classical  mechanics,  126 
closure  equations,  152 
compatibility  conditions 
for  acceleration,  363 
for  velocity,  357 

composite  rigid-body  method,  317 
composition  of  reflections  and 
rotations,  58 

condition  number,  226, 403 
conflguration  of  a  manipulator,  139 
constraint  wrenches,  185 
continuous  path,  256,  376 
operations,  465 
tracking,  492 
control  vector,  306,  340 
coordinate  transformation,  60-70 
Coriolis 

acceleration,  121 

and  centrifugal  forces,  310,  311,  317 
Couette  flow,  344 
Coulomb 

dissipation  function,  345 
friction,  285,  344 
CP,  see  continuous  path 
cross-product  matrix,  37 
curvature,  467 
derivative  w.  r.  t. 
a  parameter,  475 

derivative  w.  r.  t.  the  arc  length,  467 
parametric  representation,  473 
time-derivative,  469 
cycloidal  motion,  265 

D 

Darboux  vector,  469 
time-derivative,  470 
decoupled  manipulators,  143, 151 
decoupled  robots,  139 
Delta  Robot,  13 
Delta  robot,  13 

delta-array  (A -array),  462,  534 
Denavit-Hartenberg 
frames,  141 
notation,  139 
parameters,  143 
rotation  matrix,  145 
vector  joining  two  frame  origins,  145 
determined  system,  564 
dexterity,  25 


measures,  see  kinetostatic  performance 
indices 
DEXTRE,  5 

dextrous  hands,  see  multiflngered  hands 
dextrous  manipulation,  10 
dextrous  workspace,  222 
DH,  see  Denavit-Hartenberg 
dialytic  elimination,  204,  252 
DIESTRO 

inverse  kinematics,  414 
Jacobian,  249 
manipulator,  239, 414 
differentiation  with  respect  to 
vectors,  37 

direct  kinematic  problem 

of  parallel  manipulators,  421 
displacement  equations  of  a 
manipulator,  152 
dissipation  function,  285,  344 
duality,  185 
dynamic  systems,  1 
dynamics 

of  holonomic  systems,  507 

of  multibody  systems,  283 

of  parallel  manipulators,  512 

of  rigid  bodies,  125 

of  robotic  mechanical  systems,  507 

of  rolling  robots,  523 

of  serial  manipulators,  281 

E 

EE,  see  end-effector 
elastodynamic,  186 
end-effector,  141 
Euclidean  norm,  39 
Euler 

angles,  44,  88,  94 
equation  (for  graphs),  514 
equation  (in  mechanics),  129 
parameters,  see  Euler-Rodrigues 
parameters 

Euler’s 

formula  for  graphs,  see  Euler  equation  for 
graphs 
theorem,  36 

Euler-Lagrange  equations,  282,  284 
derived  with  the  NOC,  309,  510 
Euler-Rodrigues  parameters,  55 

F 

Eanuc  Arc  Mate 

inverse  displacement,  410 
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Fanuc  Arc  Mate  120iB,  181 
Fanuc  Robot  Arc  Mate 
characteristic  length,  248 
DH  parameters,  248 
KCI,  249 

feasible  twists,  185 

First  Law  of  Thermodynamics,  211 

flight  simulator,  422 

floating-point  operation,  25, 209,  317,  557 
flop,  see  floating-point  operation 
forward  dynamics 

algorithm  complexity,  335 
of  serial  manipulators,  281,  317 
fractal,  568 

Frenet,  see  Frenet-Serret 
Frenet-Serret 
formulas,  467 
frame,  466 
vectors,  466 
friction  forces,  343 
Frobenius  norm,  225 
fuzzy  logic,  24 

G 

genealogy  of  robotic  mechanical  systems,  1, 4 
general  architecture  of  a 
manipulator,  5 

generalized  coordinates,  284,  285,  508 
generalized  forces,  284,  285,  530 
generalized  inertia  matrix,  287, 539 
Cholesky  decomposition,  318 
factoring,  319 
time-rate  of  change,  321 
generalized  speeds,  285,  508 
Ginger,  see  Segway 
gluing  operation,  476 
grasping  matrix,  444 
gravity 

terms,  342 
wrench,  307 

H 

hand-eye  calibration,  83 
Hexa  robot,  14 
higher  kinematic  pair,  140 
holonomic  systems,  507,  508 
homogeneous  coordinates,  60 
homotopy,  376 

I 

IDP,  see  inverse  displacement  problem 
donators,  20 


inertia  dyad,  130,  283 
inertial  frame,  120, 126 
input,  1,  306 
instant  screw  axis,  110 
instrument  calibration,  8 1 
intelligent  machines,  2,  24 
intelligent  robots,  2 
invariance,  76 

inverse  displacement  problem  of 
a  general  6R  manipulator,  375 
inverse  dynamics 

of  serial  manipulators,  281 
recursive,  294 

inverse  kinematics  problem  of 
parallel  manipulators,  424 
inverse  vs.  forward  dynamics,  281 
inward  recursions,  301,  304 
ISA,  see  instant  screw  axis 
isomorphism,  33 
isotropic 

manipulator,  227 
matrix,  224,  227 
isotropy,  230 
iteration,  25, 497 

J 

Jacobian  matrix,  185 
condition  number,  226 
evaluation,  194 
invertibility,  222 
transfer  formula,  190 
joint,  140 

coordinates,  139, 143 
parameters,  143 
variables,  143 

K 

Kane’s  equations,  293 
KCI,  see  kinematic  conditioning  index 
kernel  of  a  linear  transformation,  29 
Kinemate,  115 
kinematic 
chain,  140 

conditioning  index,  225 
constraints,  307 

constraints  for  serial  manipulators,  311 
pair,  140 
kinematic  chain 

architecture  of  a  ,  141 
kinetic  energy,  126 
kinetostatic  performance  indices,  221 
kinetostatics,  185 
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L 

least-square  error,  568 
least- square  solution,  569 
Lee  vs.  Li,  377 
Lee’s  manipulator,  411 
Lee’s  procedure,  407 
left  hand,  14 
legged  robots,  17 
Li  vs.  Lee,  see  Lee  vs.  Li 
Li’s  manipulator,  see  Lee’s  manipulator 
linear  invariants,  44 
of  rotation,  46 
linear  transformations,  28 
local  structure  of  a  manipulator,  143 
locomotors,  17 
lower  kinematic  pair,  140 
LU  decomposition,  190 

M 

machine  (definitions  of),  24 
main  gauche,  see  left  hand 
maneuverability,  461 
manipulability,  222 

of  decoupled  manipulators,  254 
manipulator 

angular  velocity  matrix,  286 
architecture,  139 
configuration,  139 
dynamics,  281, 512 
general  architecture,  5 
mass  matrix,  286 
posture,  139 
twist,  286 
wrench,  286 
manipulators,  8, 139 
matrix 

norm,  225 
representation,  32 
mechanical  system,  2 
mechatronics,  25 
Mekanum  wheels,  20 
minimum-time  trajectory,  310 
mobile  wheeled  pendulums,  21 
module,  36 
moment 

of  inertia,  128 
moment  invariants,  77 
moment  of  a  line 
about  a  point,  103 
about  another  line,  133 
moment-of-inertia  matrix,  128 
momentum  screw,  130 
Motoman-EA1400N  welding  robot,  182 


motor,  115 

Mozzi-Chasles’  Theorem,  97 
MSS,  5 

multibody  system 
dynamics,  283 

Euler-Lagrange  equations,  294 
multicubic  expression,  153 
multifingered  hands,  10 
multilinear  expression,  153 
multiquadratic  expression,  153 
multiquartic  expression,  153 

N 

natural  orthogonal  complement,  282 
applied  to  holonomic  systems,  510 
applied  to  parallel  manipulators,  512 
applied  to  planar  manipulators,  330 
applied  to  rolling  robots,  524,  537 
Newton 

-Gauss  method,  493 
-Raphson  method,  84 
equation,  129 
methods,  377, 493 
Newton-Euler 
equations,  129 

Newton-Euler  equations,  130 

NOC,  see  natural  orthogonal  complement 

nonholonomic  systems,  282,  507,  523 

noninertial  base  link,  316 

nonlinear  system,  564 

norm 

also,  see  Erobenius  norm 
norm  (matrix  -),  225 
normal  component  of  a  vector,  3 1 
normality  condition,  569 
null  space  of  a  linear  transformation,  29 
numerical  conditioning,  390, 403 

O 

object-oriented  programming,  25 
Odetics  series  of  hexapods,  17 
ODW,  see  omnidirectional  wheels,  see 
omnidirectional  wheels 
off-line,  5, 158, 196 
omnidirectional  wheels,  20,  534 
dynamics,  534 
kinematics,  457 
on-line,  25 
operation  point,  142 
orientation  problem,  171 
orthogonal  complement,  308 
orthogonal  decomposition  of  a  vector,  3 1 
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orthogonal  decoupled  manipulator,  165 
orthogonal  projection,  29 
orthogonal  RRR  manipulator 
dynamics,  321,  332 
inverse  kinematics,  166, 169 
recursive  dynamics,  328 
workspace,  173 
OSU  ASV,  17 
OSU  Hexapod,  17 
output,  1 

outward  recursions,  295 
overdetermined  system,  568 

P 

Pappus-Guldinus  theorem,  253 
parallel  axes,  theorem,  129 
parallel  manipulators 

acceleration  analysis,  436 
dynamics,  512 
kinematics,  421 
velocity  analysis,  436 
parallel  robots,  12 
parametric 

path  representation,  472 
representation  of  curvature,  473 
representation  of  curvature  derivative,  475 
representation  of  torsion,  473 
representation  of  torsion  derivative,  475 
splines,  486 

path-tracking  for  arc-welding,  498 
pick-and-place  operations,  255,  256 
Plucker  coordinates 
of  a  line,  102 
transfer  formula,  104 
planar  manipulators,  212 
acceleration  analysis,  218 
displacement  analysis,  214 
dynamics,  288 
static  analysis,  220 
velocity  analysis,  216 
platform  manipulators,  424, 431,  512 
polar-decomposition  theorem,  222 
polynomial  interpolation 

with  3-4-5  polynomial,  258 
with  4-5-6-7  polynomial,  262 
pose 

array,  106 
of  a  rigid  body,  105 
positioning  problem,  154 
posture  of  a  manipulator,  139 
PPO,  see  pick-and-place  operations 
principal  axes  of  inertia,  128 
principal  moments  of  inertia,  128 


Principle  of  Virtual  Work,  211 
prismatic  pair,  140, 199 
programmable  robot,  2 
projection,  29 
theorem,  557 
Puma  robot,  142, 143, 163 
DH  parameters,  143 
inverse  kinematics,  154 
workspace,  164 
pure  reflection,  30 

Q 

quaternions,  57 

R 

Raghavan  and  Roth’s  procedure,  377 
Raghavan-Roth’s  procedure,  390 
range  of  a  linear  transformation,  29,  85 
Rayleigh  dissipation  function,  see  dissipation 
function 

real-time,  25,  281 
reciprocal  bases,  82, 193,  522 
reciprocal  product,  125 
recursion,  25 
redundant  sensing,  8 1 
References,  571 
reflection,  30,  378,  384 

composition  with  rotations,  58 
regional  structure  of  a  manipulator,  143 
revolute  pair,  140 
rheonomic  systems,  508 
robot  design,  186 
robotic  hands,  10 
robotic  mechanical  systems,  vii,  1 
Rodrigues,  see  Euler-Rodrigues 
vector,  88 
rolling  robots 
dynamics,  523 
kinematics,  450 
rotating  pair,  140 
rotation,  33 
rotation  matrix,  41 

exponential  representation,  43 
run-time,  25 

Runge-Kutta  methods,  341 
RVS,  viii,  257 

S 

SARAH,  1 1 

Schonflies-motion  generators,  16 
scleronomic  systems,  508 
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screw 

amplitude,  99 
axis,  99 
motion,  95 
pitch,  99 
S  eg  way,  21 
self-inverse,  3 1 
serial  manipulators 

acceleration  analysis,  206 
dynamics,  281 
kinematics,  140 
statics,  210 
velocity  analysis,  186 
workspace,  203 
service  angle,  222 
similarity  transformations,  70 
simple  manipulation,  10 
simulation,  339 

singular- value  decomposition,  224 
singular- values,  224 
singularities,  185 

singularity  analysis  of  decoupled  manipulators, 
199 

sliding  pair,  140 
SPDM,  5 

spherical  wrist,  142, 172, 173 
workspace,  174 
spline(s),  269 

interpolation  of  4-5-6-7  polynomial, 

275 

natural,  273 

nonparametric,  270, 486 
parametric,  486 
periodic,  270 

square  root  of  a  matrix,  56 

Star  robot,  14 

state 

of  a  dynamical  system,  306 
of  parallel  manipulators,  519 
of  serial  manipulators,  306,  340 
variable,  285,  306,  340 
variable  equations,  340 
vector,  306 
static  analysis 

of  rigid  bodies,  121 
of  serial  manipulators,  210 
static,  conservative  conditions,  185 
stationary  point,  570 
Steiner’s  Theorem 
in  dyad  form,  131 
Steiner,  theorem,  129 
Stewart  platform,  see  Stewart-Gough 
platform 


Stewart-Gough  platform,  xiii,  424 
direct  kinematics,  422 
leg  kinematics,  424 
structural  design,  186 
structure  of  mechanical  systems,  9 
structured  environment,  3 
Sutherland,  Sprout  &  Assocs.  Hexapod,  17 
Swedish  wheels,  20 
system,  1 

T 

telemanipulators,  5 
tensors,  27,  305,  306 
Titan  series  of  quadrupeds,  17 
torsion,  467 

derivative  w.  r.  t. 
a  parameter,  475 

derivative  w.  r.  t.  the  arc  length,  467 
parametric  representation,  473 
time-derivative,  469 
trace  of  a  square  matrix,  45 
trajectories  with  via  poses,  267 
trajectory  planning,  255, 465 
truncation  error,  341 
Trussarm,  14,  15 
TU  Munich  Hand,  1 1 
TU  Munich  Hexapod,  18 
twist.  111 

axis  coordinates,  115 
of  a  rigid  body,  114 
ray  coordinates,  115 
transfer  formula,  117 
twist- shape  relations,  308 
for  serial  manipulators,  311 

U 

unimodular  group  (of  matrices),  104 
unstructured  environment,  3 

V 

vector  of  a  3  X  3  matrix,  45 
vector  space,  28 
velocity  analysis 

of  parallel  manipulators,  436 
of  rolling  robots,  452 
of  serial  manipulators,  186 
via  poses,  268 

virtual  work,  see  Principle  of  Virtual  Work 
viscosity  coefficient,  343 
viscous  forces,  343 
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W 

walking  machines 
kinematics,  447 
leg  architecture,  448-450 
walking  stick,  17 
weighting  matrix,  568 
wheeled  robots,  19 


workspace  of  positioning  manipulators, 
202 

wrench 

acting  on  a  rigid  body,  122 
axis,  123 
pitch,  123 

transfer  formula,  125 


